diff --git a/README.md b/README.md index 5f1767b..665a3b3 100644 --- a/README.md +++ b/README.md @@ -9,16 +9,23 @@ #### 安装教程 -1. xxxx -2. xxxx +1. chmod +x build.sh +2. ./build.sh 3. xxxx #### 使用说明 +<<<<<<< Updated upstream 在最外层目录 catkin_make 可能需要 source ./devel/setup.bash 1. roslaunch librviz_tutorial laser.launch 2. xxxx +======= +在工作目录下(src上一层) +记得source(可能需要) +1. source ./devel/setup.bash +2. roslaunch point_visual laser.launch +>>>>>>> Stashed changes 3. xxxx #### 参与贡献 diff --git a/build.sh b/build.sh new file mode 100755 index 0000000..5a8e36b --- /dev/null +++ b/build.sh @@ -0,0 +1,28 @@ +#!/bin/bash + +# 创建工作目录 +mkdir ~/PointVisual + +cd .. +#将当前src移动到PointVisual +cp -r src ~/PointVisual + +# 切换到工作目录 +cd ~/PointVisual + +# 在工作目录make +catkin_make + +if [ $? -eq 0 ]; then + +# 执行source命令 + source ./devel/setup.bash + +# 运行其他命令 +# roslaunch point_visual laser.launch + roscore & + sleep 2 + rosrun point_visual myviz +else + exit 1 +fi diff --git a/librviz_tutorial/launch/laser.launch b/librviz_tutorial/launch/laser.launch deleted file mode 100644 index 0a19978..0000000 --- a/librviz_tutorial/launch/laser.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/librviz_tutorial/src/myviz.cpp b/librviz_tutorial/src/myviz.cpp deleted file mode 100755 index 547cc8a..0000000 --- a/librviz_tutorial/src/myviz.cpp +++ /dev/null @@ -1,1105 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#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) - : QWidget( parent ) -{ - - nh=ros::NodeHandle("~"); - sub=nh.subscribe("/test",1000,&MyViz::subCallback,this); - pub=nh.advertise("/test_pub",1000); - pub_thread=std::thread(&MyViz::pubThread,this); - - // 全局变量存放处 - // 颜色名称，可添加，待设置rgb类型设置颜色 - color_name = { - "white","red","green","black","blue","yellow", - "pink","purple","skyblue","lightgreen","orange", - }; - // 颜色路径名 - color_icon_filename = QString("../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; - - dlg = new LoginDialog(this); - landed = 0; - cur_fixed_frame = "world"; - - // 网格结构 - QGridLayout* controls_layout = new QGridLayout(); - int ctl_index = 0; - - // 登录按钮 - QPushButton* login_button = new QPushButton("登录"); - QPushButton* quit_login_button = new QPushButton("退出"); - - //select按钮 - QPushButton* selection_button = new QPushButton("选择坐标点"); - QPushButton* move_camera_button = new QPushButton("移动"); - - controls_layout->addWidget( login_button, ctl_index, 0); - controls_layout->addWidget( quit_login_button, ctl_index++, 1); - - - // offline 布局 - // QPushButton setting!//////////////////////////////////////////////////////////////////////////////////////////////// - QPushButton* play = new QPushButton("播放", this); - QPushButton* pause = new QPushButton("暂停", this); - QPushButton* but_save = new QPushButton("保存一帧", this);// buttonn: save one frame - QPushButton* but_saveMul = new QPushButton("保存多帧", this);// button: save multiple frames - QPushButton* but_file = new QPushButton("打开文件", this);// button: select file - - // QLabel setting!///////////////////////////////////////////////////////////////////////////////////////////////////// - QLabel* label_speed = new QLabel("播放速度:");//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(false); - - // setting button size!//////////////////////////////////////////////////////////////////////////////////////////////// - play->setFixedSize(120, 30); - pause->setFixedSize(120, 30); - but_save->setFixedSize(120, 30); - but_saveMul->setFixedSize(120, 30); - but_file->setFixedSize(120, 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( move_camera_button, ctl_index, 0); - controls_layout->addWidget( selection_button, ctl_index++ ,1); - - 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("参考坐标系:"); - QCheckBox *axes_checkbox = new QCheckBox("", this); - // QComboBox * axesBox = new QComboBox(); - // axesBox->addItem("不显示"); - // axesBox->addItem("显示"); - - // 选择圆圈范围 - QLabel* label_circle = new QLabel("参考圆(m): "); - QLineEdit* circleLineEdit = new QLineEdit(); - circleLineEdit->setPlaceholderText("请输入数字"); - circleLineEdit->setFixedWidth(120); - - controls_layout->addWidget(label_axes, ctl_index,0); - controls_layout->addWidget(axes_checkbox, ctl_index++,1); - controls_layout->addWidget(label_circle, ctl_index,0); - controls_layout->addWidget(circleLineEdit, ctl_index++,1); - - // 第二个layout - //QGridLayout* display_layout = new QGridLayout(); - //int display_index = 0; - - // Tree 全局变量存放地方 - Tree_Display(controls_layout, ctl_index++); - - - // 待解锁子组件设置父组件，方便寻找，登陆后解锁可编辑属性 - login_button->setObjectName("login_button"); - login_button->setParent(this); - quit_login_button->setObjectName("quit_login_button"); - quit_login_button->setParent(this); - quit_login_button->setEnabled(false); - - - but_file->setObjectName("open_file"); - but_file->setParent(this); - progress->setObjectName("bag_progress"); - progress->setParent(this); - progress->setEnabled(false); - - // controls_layout->setRowStretch(3, 0); - // Make signal/slot connections. 进行信号/插槽连接 - - // 增加登录按钮 - connect(login_button, SIGNAL(clicked()), this, SLOT(login_button_clicked())); - // 增加退出按钮 - connect(quit_login_button, SIGNAL(clicked()), this, SLOT(quit_login_button_clicked())); - - // 增加切换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(axes_checkbox, SIGNAL(stateChanged(int)), this, SLOT(AxesDisplayChanged(int))); - // 增加圆圈变换 - connect(circleLineEdit, SIGNAL(textChanged(const QString&)), this, SLOT(CircleDisplayChanged(const QString&))); - //off - connect(progress,&QSlider::valueChanged,this,[=](){// 变动会触发 - if(!ismoved){ - ischanged = true;// “戳”这一个动作会使ischange变成true - // qDebug()<<"CLICK"<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水平 - - // Next we initialize the main RViz classes. - // - // The VisualizationManager is the container for Display objects, - // holds the main Ogre scene, holds the ViewController, etc. It is - // very central and we will probably need one in every usage of - // librviz. - manager_ = new rviz::VisualizationManager( render_panel_ ); // 是实现rviz功能的类 - // 初始化camera 这行代码实现放大 缩小 平移等操作 - render_panel_->initialize( manager_->getSceneManager(), manager_ ); - manager_->setFixedFrame("laser"); - // 初始化tool_manager_ - tool_manager_ = manager_->getToolManager(); - - // 在初始化之后再进行更改操作 - manager_->initialize(); - manager_->startUpdate(); - - manager_->setFixedFrame(cur_fixed_frame); - // qDebug()<< cur_fixed_frame; - - // 创建select panel - QLabel *select_label = new QLabel("坐标点显示:"); - tree_widget_ = new rviz::PropertyTreeWidget(); - tree_widget_->setModel(manager_->getSelectionManager()->getPropertyModel()); - ctl_index++; - controls_layout->addWidget(select_label, ctl_index++, 0); - // controls_layout->addWidget(tree_widget_, ctl_index++, 0); - controls_layout->addWidget(tree_widget_, ctl_index++, 0, 1, 2); - - // 设置网格某一行伸缩比例,放最后一行 - // controls_layout->setRowStretch(--ctl_index, 6); - - // Create a Grid display. - grid_ = manager_->createDisplay( "rviz/Grid", "adjustable grid", true ); - ROS_ASSERT( grid_ != NULL ); - - // cloud = manager_->createDisplay( "rviz/PointCloud2", "point cloud", true ); - // ROS_ASSERT( cloud != NULL ); - - laser_ = manager_->createDisplay( "rviz/LaserScan", "Qlaser", true ); - ROS_ASSERT( laser_ ); - // 新增雷达 - laser_->subProp("Topic")->setValue("/scan"); - laser_->subProp("Size (Pixels)")->setValue("2"); - laser_->subProp("Color Transformer")->setValue("FlatColor"); - laser_->subProp("Color")->setValue(color_name[1]); - // cloud->subProp("Topic")->setValue("/pose_graph/octree"); - // cloud->subProp("Style")->setValue("Points"); - // cloud->subProp("Size (Pixels)")->setValue("2"); - // cloud->subProp("Color Transformer")->setValue("Intensity"); - // cloud->subProp("Invert Rainbow")->setValue("true"); - // cloud->subProp("Decay Time")->setValue("1"); - // Configure the GridDisplay the way we like it. - grid_->subProp( "Line Style" )->setValue( "Billboards" ); - grid_->subProp( "Color" )->setValue( QColor( Qt::white ) ); - - // 新增坐标系 - axes_display= manager_->createDisplay("rviz/Axes", "Axes", false); - ROS_ASSERT( axes_display != NULL ); - axes_display->subProp("Length")->setValue(0.2); // 设置透明度 - axes_display->subProp("Radius")->setValue(0.05); // 设置坐标系的缩放大小 - // 新增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( controls_layout ); - main_layout->addWidget( render_panel_,2); // 存放rviz的位置, stretch 拉伸系数 - setLayout( main_layout ); - -} - -void MyViz::Tree_Display(QGridLayout* controls_layout, int index){ - QTreeWidget *menu = new QTreeWidget(); - // menu->setColumnCount(2); - // 列头自适应大小 - 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); - global->setExpanded(true); - - // fix frame - QTreeWidgetItem *fixed_frame = new QTreeWidgetItem(global, QStringList("固定参考系")); - - QLineEdit* fixed_frame_text = new QLineEdit(); - fixed_frame_text->setText("world"); - fixed_frame_text->setStyleSheet("background:transparent;border-width:0;border-style:inset"); - // 初始化固定坐标系 - cur_fixed_frame = fixed_frame_text->text(); - // 设置事件 - 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("背景颜色")); - 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); - // global->setTextAlignment(1, 2); // (列数，对齐方式) - // grid - 一层树 - QTreeWidgetItem * grid = new QTreeWidgetItem(menu, QStringList("栅格")); - // 二层 - QTreeWidgetItem *grid_color = new QTreeWidgetItem(grid, QStringList("颜色")); - QComboBox * grid_color_cbox = 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); - 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); - - // grid cell size - QTreeWidgetItem *grid_cell = new QTreeWidgetItem(grid, QStringList("网格大小")); - 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("线条粗细")); - 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->setSliderPosition(1); - menu->setItemWidget(grid_thickness, 1, thickness_slider); - - // laser - 一层树 - QTreeWidgetItem * laser = new QTreeWidgetItem(menu, QStringList("雷达")); - // 二层 - QTreeWidgetItem *laser_topic = new QTreeWidgetItem(laser, QStringList("话题")); - // Topic_Laser 输入雷达 - QLineEdit* laser_topic_text = new QLineEdit(); - - // 方便寻找，登陆后解锁 - laser_topic_text->setObjectName("laser_topic"); - laser_topic_text->setParent(this); - laser_topic_text->setEnabled(false); - // 增加雷达话题 - 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("大小")); - 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("颜色")); - QComboBox * laser_color_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); - laser_color_comboBox->addItem(color_icon, color_name[i]); - } - // 设置初始化雷达颜色为红色 - laser_color_comboBox->setCurrentIndex(1); - - // 增加颜色变换 - connect(laser_color_comboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(LaserColorChanged(int))); - menu->setItemWidget(laser_color, 1, laser_color_comboBox); - - //controls_layout->addWidget(menu, index, 0 ); - controls_layout->addWidget(menu, index, 0, 1, 2); -} - - -void MyViz::quit_login_button_clicked(){ - landed = 0; - // 退出设置登录按钮可用 - QWidget * login_button = this->findChild("login_button"); - if (login_button) { - login_button->setEnabled(true); - }else{ - qDebug() << "login_button组件未找到" ; - } - // 退出设置退出按钮不可用 - QWidget * quit_button = this->findChild("quit_login_button"); - if (quit_button) { - quit_button->setEnabled(false); - }else{ - qDebug() << "quit_login_button组件未找到" ; - } - // 退出设置其他组件不可用 - if(dlg != NULL){ - for (int i=0; i< dlg->setting_name.size(); i++){ - QWidget *foundChild = this->findChild(dlg->setting_name[i]); - if (foundChild) { - // 找到了指定的子组件 在这里可以对子组件进行操作 - foundChild->setEnabled(false); - }else{ - qDebug()<<"第" << i+1 <<"组件未找到" ; - } - } - } -} - -void MyViz::login_button_clicked(){ - dlg->setWindowTitle(tr("登录")); - // dlg->setFixedSize(400, 300); - // dlg->open();// 非模态级别对话框 - if(dlg->exec() == QDialog::Accepted){ - // 登录成功后 - landed = 1; - qDebug() << "login accepted"; - // 登录设置其他组件和退出可用 - QWidget * quit_button = this->findChild("quit_login_button"); - if (quit_button) { - quit_button->setEnabled(true); - }else{ - qDebug() << "quit_login_button组件未找到" ; - } - // 登录设置登录不可用 - QWidget * login_button = this->findChild("login_button"); - if (login_button) { - login_button->setEnabled(false); - }else{ - qDebug()<<"login_button组件未找到" ; - } - }else{ - // 登录失败 - qDebug() << "login refused"; - } -} - -void MyViz::PlayCircle(double 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_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 = cur_fixed_frame.toStdString(); - // 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(const QString& text){ - double index = text.toDouble(); - if(index <= 0){ - marker_->setValue(false); - }else - { - marker_->setValue(true); - std::thread ThreadCircle(&MyViz::PlayCircle, this, index);// 新开线程执行操作 - ThreadCircle.detach();// 执行完成后自动回收资源 - } -} - -void MyViz::AxesDisplayChanged(int stage) -{ - // 0未被选中，1被部分选中，2 被选中 - switch (stage) - { - case 0: // 未被选中 - if (axes_display != NULL) - axes_display->setValue(false); - break; - - case 2: // 被选择 - 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; - QColor new_color = parseColor(t); - if(new_color.isValid()){ - // rviz源码中修改背景颜色的方式 - render_panel_->setBackgroundColor(qtToOgre(new_color)); - manager_->queueRender(); - } - else - { - ROS_INFO_STREAM("Color invalid !!!"); - } - }else{ - ROS_INFO_STREAM("manager_ failed !!!"); - } -} - - -void MyViz::LaserColorChanged(int index){ - if(laser_!=NULL) laser_->subProp("Color")->setValue(color_name[index]); - -} - -void MyViz::GridColorChanged(int index){ - if(grid_!=NULL) grid_->subProp("Color")->setValue(color_name[index]); -} - -// Destructor. -MyViz::~MyViz() -{ - delete manager_; -} - -// This function is a Qt slot connected to a QSlider's valueChanged() -// signal. It sets the line thickness of the grid by changing the -// grid's "Line Width" property. -void MyViz::setThickness( int thickness_percent ) -{ - if( grid_ != NULL ) - { - grid_->subProp( "Line Style" )->subProp( "Line Width" )->setValue( thickness_percent / 100.0f ); - } -} - -// This function is a Qt slot connected to a QSlider's valueChanged() -// signal. It sets the cell size of the grid by changing the grid's -// "Cell Size" Property. -void MyViz::setCellSize( int cell_size_percent ) -{ - if( grid_ != NULL ) - { - grid_->subProp( "Cell Size" )->setValue( cell_size_percent / 10.0f ); - } -} - -void MyViz::setCloudTopic(const QString &newTopic){ - if(cloud!=NULL){ - cloud->subProp( "Topic" )->setValue(newTopic); - //ROS_INFO_STREAM("cloud topic changed to => "<subProp( "Topic" )->setValue(newTopic); - ROS_INFO_STREAM("laser topic changed to => "<setEnabled(true); - // laser->subProp( "Color" )->setValue( QColor( Qt::yellow ) ); - // manager_->setFixedFrame("laser"); - manager_->startUpdate(); -} - -void MyViz::setFixedFrame(const QString & fix_frame){ - manager_->setFixedFrame(fix_frame); - cur_fixed_frame = fix_frame; - manager_->startUpdate(); -} - -void MyViz::setCloudSize(int cloudsize){ - if(cloud!=NULL){ - cloud->subProp("Size (Pixels)")->setValue(cloudsize); - } -} - -void MyViz::setLaserSize(int lasersize){ - if(laser_!=NULL){ - laser_->subProp("Size (Pixels)")->setValue(lasersize); - } -} - -void MyViz::subCallback(const std_msgs::String& msg){ - ROS_INFO_STREAM("receive message!"); -} - -void MyViz::pubThread(){ - while(ros::ok()){ - 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("scan", 5);// "topic" - // sleep time - - bag.open(pcdFilePathStr, rosbag::bagmode::Read); - - rosbag::View view1(bag, rosbag::TopicQuery("/scan")); - view = &view1; - std::cout<size()<begin(); it != view->end(); ++it)// 用于预先存储bag包的信息 - { - auto m = *it; - sensor_msgs::LaserScan::ConstPtr input = m.instantiate(); - indx.push_back(input); - } - stored = false; - indxsize = indx.size(); - bag.close(); - qDebug()<=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()<setSliderPosition(ceil(countbag/indxsize*100)); - qDebug()<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(countbagsliderPosition()<=100 && countbagvalue/indxsize*100<=100) - { - qDebug()<<"pause"; - progress->setSliderPosition(ceil(countbagvalue/indxsize*100)); - qDebug()<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"; - } -} \ No newline at end of file diff --git a/librviz_tutorial/CHANGELOG.rst b/point_visual/CHANGELOG.rst similarity index 100% rename from librviz_tutorial/CHANGELOG.rst rename to point_visual/CHANGELOG.rst diff --git a/librviz_tutorial/CMakeLists.txt b/point_visual/CMakeLists.txt similarity index 83% rename from librviz_tutorial/CMakeLists.txt rename to point_visual/CMakeLists.txt index 5d360fe..d3a9c55 100644 --- a/librviz_tutorial/CMakeLists.txt +++ b/point_visual/CMakeLists.txt @@ -4,7 +4,7 @@ ## ## First start with some standard catkin stuff. cmake_minimum_required(VERSION 3.0.2) -project(librviz_tutorial) +project(point_visual) set(SDK_PATH "./sdk/") @@ -17,7 +17,15 @@ FILE(GLOB SDK_SRC find_package(catkin REQUIRED COMPONENTS rviz roscpp std_msgs rosconsole sensor_msgs # node.cpp -rosbag +rosbag +pcl_ros +) + +find_package(PCL REQUIRED) +include_directories( + include + ${catkin_INCLUDE_DIRS} + /usr/include/pcl-1.8 ) @@ -68,8 +76,8 @@ set(SRC_FILES add_executable(myviz ${SRC_FILES} ) -add_executable(rplidarNode src/node.cpp ${SDK_SRC}) # 创建启动laser -target_link_libraries(rplidarNode ${catkin_LIBRARIES}) +# add_executable(rplidarNode src/node.cpp ${SDK_SRC}) # 创建启动laser +# target_link_libraries(rplidarNode ${catkin_LIBRARIES}) target_link_libraries(myviz -lcrypto) @@ -85,11 +93,11 @@ target_link_libraries(myviz ${QT_LIBRARIES} ${catkin_LIBRARIES}) ## Install install(TARGETS myviz DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) -install(TARGETS rplidarNode - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +# install(TARGETS rplidarNode +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) install(DIRECTORY launch sdk DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} diff --git a/point_visual/launch/laser.launch b/point_visual/launch/laser.launch new file mode 100644 index 0000000..425c158 --- /dev/null +++ b/point_visual/launch/laser.launch @@ -0,0 +1,10 @@ + + + + + + \ No newline at end of file diff --git a/librviz_tutorial/launch/rplidar.launch b/point_visual/launch/rplidar.launch similarity index 81% rename from librviz_tutorial/launch/rplidar.launch rename to point_visual/launch/rplidar.launch index 0b2c7e2..1c88615 100644 --- a/librviz_tutorial/launch/rplidar.launch +++ b/point_visual/launch/rplidar.launch @@ -1,5 +1,5 @@ - + diff --git a/librviz_tutorial/package.xml b/point_visual/package.xml similarity index 96% rename from librviz_tutorial/package.xml rename to point_visual/package.xml index b6183b9..a1961ef 100644 --- a/librviz_tutorial/package.xml +++ b/point_visual/package.xml @@ -1,5 +1,5 @@ - librviz_tutorial + point_visual 0.11.0 Tutorial showing how to compile your own C++ program with RViz displays and features. diff --git a/librviz_tutorial/rosdoc.yaml b/point_visual/rosdoc.yaml similarity index 100% rename from librviz_tutorial/rosdoc.yaml rename to point_visual/rosdoc.yaml diff --git a/librviz_tutorial/sdk/README.txt b/point_visual/sdk/README.txt similarity index 100% rename from librviz_tutorial/sdk/README.txt rename to point_visual/sdk/README.txt diff --git a/librviz_tutorial/sdk/include/rplidar.h b/point_visual/sdk/include/rplidar.h similarity index 100% rename from librviz_tutorial/sdk/include/rplidar.h rename to point_visual/sdk/include/rplidar.h diff --git a/librviz_tutorial/sdk/include/rplidar_cmd.h b/point_visual/sdk/include/rplidar_cmd.h similarity index 100% rename from librviz_tutorial/sdk/include/rplidar_cmd.h rename to point_visual/sdk/include/rplidar_cmd.h diff --git a/librviz_tutorial/sdk/include/rplidar_driver.h b/point_visual/sdk/include/rplidar_driver.h similarity index 100% rename from librviz_tutorial/sdk/include/rplidar_driver.h rename to point_visual/sdk/include/rplidar_driver.h diff --git a/librviz_tutorial/sdk/include/rplidar_protocol.h b/point_visual/sdk/include/rplidar_protocol.h similarity index 100% rename from librviz_tutorial/sdk/include/rplidar_protocol.h rename to point_visual/sdk/include/rplidar_protocol.h diff --git a/librviz_tutorial/sdk/include/rptypes.h b/point_visual/sdk/include/rptypes.h similarity index 100% rename from librviz_tutorial/sdk/include/rptypes.h rename to point_visual/sdk/include/rptypes.h diff --git a/librviz_tutorial/sdk/src/arch/linux/arch_linux.h b/point_visual/sdk/src/arch/linux/arch_linux.h similarity index 100% rename from librviz_tutorial/sdk/src/arch/linux/arch_linux.h rename to point_visual/sdk/src/arch/linux/arch_linux.h diff --git a/librviz_tutorial/sdk/src/arch/linux/net_serial.cpp b/point_visual/sdk/src/arch/linux/net_serial.cpp similarity index 100% rename from librviz_tutorial/sdk/src/arch/linux/net_serial.cpp rename to point_visual/sdk/src/arch/linux/net_serial.cpp diff --git a/librviz_tutorial/sdk/src/arch/linux/net_serial.h b/point_visual/sdk/src/arch/linux/net_serial.h similarity index 100% rename from librviz_tutorial/sdk/src/arch/linux/net_serial.h rename to point_visual/sdk/src/arch/linux/net_serial.h diff --git a/librviz_tutorial/sdk/src/arch/linux/net_socket.cpp b/point_visual/sdk/src/arch/linux/net_socket.cpp similarity index 100% rename from librviz_tutorial/sdk/src/arch/linux/net_socket.cpp rename to point_visual/sdk/src/arch/linux/net_socket.cpp diff --git a/librviz_tutorial/sdk/src/arch/linux/thread.hpp b/point_visual/sdk/src/arch/linux/thread.hpp similarity index 100% rename from librviz_tutorial/sdk/src/arch/linux/thread.hpp rename to point_visual/sdk/src/arch/linux/thread.hpp diff --git a/librviz_tutorial/sdk/src/arch/linux/timer.cpp b/point_visual/sdk/src/arch/linux/timer.cpp similarity index 100% rename from librviz_tutorial/sdk/src/arch/linux/timer.cpp rename to point_visual/sdk/src/arch/linux/timer.cpp diff --git a/librviz_tutorial/sdk/src/arch/linux/timer.h b/point_visual/sdk/src/arch/linux/timer.h similarity index 100% rename from librviz_tutorial/sdk/src/arch/linux/timer.h rename to point_visual/sdk/src/arch/linux/timer.h diff --git a/librviz_tutorial/sdk/src/arch/macOS/arch_macOS.h b/point_visual/sdk/src/arch/macOS/arch_macOS.h similarity index 100% rename from librviz_tutorial/sdk/src/arch/macOS/arch_macOS.h rename to point_visual/sdk/src/arch/macOS/arch_macOS.h diff --git a/librviz_tutorial/sdk/src/arch/macOS/net_serial.cpp b/point_visual/sdk/src/arch/macOS/net_serial.cpp similarity index 100% rename from librviz_tutorial/sdk/src/arch/macOS/net_serial.cpp rename to point_visual/sdk/src/arch/macOS/net_serial.cpp diff --git a/librviz_tutorial/sdk/src/arch/macOS/net_serial.h b/point_visual/sdk/src/arch/macOS/net_serial.h similarity index 100% rename from librviz_tutorial/sdk/src/arch/macOS/net_serial.h rename to point_visual/sdk/src/arch/macOS/net_serial.h diff --git a/librviz_tutorial/sdk/src/arch/macOS/net_socket.cpp b/point_visual/sdk/src/arch/macOS/net_socket.cpp similarity index 100% rename from librviz_tutorial/sdk/src/arch/macOS/net_socket.cpp rename to point_visual/sdk/src/arch/macOS/net_socket.cpp diff --git a/librviz_tutorial/sdk/src/arch/macOS/thread.hpp b/point_visual/sdk/src/arch/macOS/thread.hpp similarity index 100% rename from librviz_tutorial/sdk/src/arch/macOS/thread.hpp rename to point_visual/sdk/src/arch/macOS/thread.hpp diff --git a/librviz_tutorial/sdk/src/arch/macOS/timer.cpp b/point_visual/sdk/src/arch/macOS/timer.cpp similarity index 100% rename from librviz_tutorial/sdk/src/arch/macOS/timer.cpp rename to point_visual/sdk/src/arch/macOS/timer.cpp diff --git a/librviz_tutorial/sdk/src/arch/macOS/timer.h b/point_visual/sdk/src/arch/macOS/timer.h similarity index 100% rename from librviz_tutorial/sdk/src/arch/macOS/timer.h rename to point_visual/sdk/src/arch/macOS/timer.h diff --git a/librviz_tutorial/sdk/src/arch/win32/arch_win32.h b/point_visual/sdk/src/arch/win32/arch_win32.h similarity index 100% rename from librviz_tutorial/sdk/src/arch/win32/arch_win32.h rename to point_visual/sdk/src/arch/win32/arch_win32.h diff --git a/librviz_tutorial/sdk/src/arch/win32/net_serial.cpp b/point_visual/sdk/src/arch/win32/net_serial.cpp similarity index 100% rename from librviz_tutorial/sdk/src/arch/win32/net_serial.cpp rename to point_visual/sdk/src/arch/win32/net_serial.cpp diff --git a/librviz_tutorial/sdk/src/arch/win32/net_serial.h b/point_visual/sdk/src/arch/win32/net_serial.h similarity index 100% rename from librviz_tutorial/sdk/src/arch/win32/net_serial.h rename to point_visual/sdk/src/arch/win32/net_serial.h diff --git a/librviz_tutorial/sdk/src/arch/win32/net_socket.cpp b/point_visual/sdk/src/arch/win32/net_socket.cpp similarity index 100% rename from librviz_tutorial/sdk/src/arch/win32/net_socket.cpp rename to point_visual/sdk/src/arch/win32/net_socket.cpp diff --git a/librviz_tutorial/sdk/src/arch/win32/timer.cpp b/point_visual/sdk/src/arch/win32/timer.cpp similarity index 100% rename from librviz_tutorial/sdk/src/arch/win32/timer.cpp rename to point_visual/sdk/src/arch/win32/timer.cpp diff --git a/librviz_tutorial/sdk/src/arch/win32/timer.h b/point_visual/sdk/src/arch/win32/timer.h similarity index 100% rename from librviz_tutorial/sdk/src/arch/win32/timer.h rename to point_visual/sdk/src/arch/win32/timer.h diff --git a/librviz_tutorial/sdk/src/arch/win32/winthread.hpp b/point_visual/sdk/src/arch/win32/winthread.hpp similarity index 100% rename from librviz_tutorial/sdk/src/arch/win32/winthread.hpp rename to point_visual/sdk/src/arch/win32/winthread.hpp diff --git a/librviz_tutorial/sdk/src/hal/abs_rxtx.h b/point_visual/sdk/src/hal/abs_rxtx.h similarity index 100% rename from librviz_tutorial/sdk/src/hal/abs_rxtx.h rename to point_visual/sdk/src/hal/abs_rxtx.h diff --git a/librviz_tutorial/sdk/src/hal/assert.h b/point_visual/sdk/src/hal/assert.h similarity index 100% rename from librviz_tutorial/sdk/src/hal/assert.h rename to point_visual/sdk/src/hal/assert.h diff --git a/librviz_tutorial/sdk/src/hal/byteops.h b/point_visual/sdk/src/hal/byteops.h similarity index 100% rename from librviz_tutorial/sdk/src/hal/byteops.h rename to point_visual/sdk/src/hal/byteops.h diff --git a/librviz_tutorial/sdk/src/hal/event.h b/point_visual/sdk/src/hal/event.h similarity index 100% rename from librviz_tutorial/sdk/src/hal/event.h rename to point_visual/sdk/src/hal/event.h diff --git a/librviz_tutorial/sdk/src/hal/locker.h b/point_visual/sdk/src/hal/locker.h similarity index 100% rename from librviz_tutorial/sdk/src/hal/locker.h rename to point_visual/sdk/src/hal/locker.h diff --git a/librviz_tutorial/sdk/src/hal/socket.h b/point_visual/sdk/src/hal/socket.h similarity index 100% rename from librviz_tutorial/sdk/src/hal/socket.h rename to point_visual/sdk/src/hal/socket.h diff --git a/librviz_tutorial/sdk/src/hal/thread.cpp b/point_visual/sdk/src/hal/thread.cpp similarity index 100% rename from librviz_tutorial/sdk/src/hal/thread.cpp rename to point_visual/sdk/src/hal/thread.cpp diff --git a/librviz_tutorial/sdk/src/hal/thread.h b/point_visual/sdk/src/hal/thread.h similarity index 100% rename from librviz_tutorial/sdk/src/hal/thread.h rename to point_visual/sdk/src/hal/thread.h diff --git a/librviz_tutorial/sdk/src/hal/types.h b/point_visual/sdk/src/hal/types.h similarity index 100% rename from librviz_tutorial/sdk/src/hal/types.h rename to point_visual/sdk/src/hal/types.h diff --git a/librviz_tutorial/sdk/src/hal/util.h b/point_visual/sdk/src/hal/util.h similarity index 100% rename from librviz_tutorial/sdk/src/hal/util.h rename to point_visual/sdk/src/hal/util.h diff --git a/librviz_tutorial/sdk/src/rplidar_driver.cpp b/point_visual/sdk/src/rplidar_driver.cpp similarity index 100% rename from librviz_tutorial/sdk/src/rplidar_driver.cpp rename to point_visual/sdk/src/rplidar_driver.cpp diff --git a/librviz_tutorial/sdk/src/rplidar_driver_TCP.h b/point_visual/sdk/src/rplidar_driver_TCP.h similarity index 100% rename from librviz_tutorial/sdk/src/rplidar_driver_TCP.h rename to point_visual/sdk/src/rplidar_driver_TCP.h diff --git a/librviz_tutorial/sdk/src/rplidar_driver_impl.h b/point_visual/sdk/src/rplidar_driver_impl.h similarity index 100% rename from librviz_tutorial/sdk/src/rplidar_driver_impl.h rename to point_visual/sdk/src/rplidar_driver_impl.h diff --git a/librviz_tutorial/sdk/src/rplidar_driver_serial.h b/point_visual/sdk/src/rplidar_driver_serial.h similarity index 100% rename from librviz_tutorial/sdk/src/rplidar_driver_serial.h rename to point_visual/sdk/src/rplidar_driver_serial.h diff --git a/librviz_tutorial/sdk/src/sdkcommon.h b/point_visual/sdk/src/sdkcommon.h similarity index 100% rename from librviz_tutorial/sdk/src/sdkcommon.h rename to point_visual/sdk/src/sdkcommon.h diff --git a/point_visual/sources/images/button/approach.png b/point_visual/sources/images/button/approach.png new file mode 100644 index 0000000..910bda2 Binary files /dev/null and b/point_visual/sources/images/button/approach.png differ diff --git a/point_visual/sources/images/button/back.png b/point_visual/sources/images/button/back.png new file mode 100644 index 0000000..9e89b4f Binary files /dev/null and b/point_visual/sources/images/button/back.png differ diff --git a/point_visual/sources/images/button/pause.png b/point_visual/sources/images/button/pause.png new file mode 100644 index 0000000..a18c404 Binary files /dev/null and b/point_visual/sources/images/button/pause.png differ diff --git a/point_visual/sources/images/button/play.png b/point_visual/sources/images/button/play.png new file mode 100644 index 0000000..1e3cd8d Binary files /dev/null and b/point_visual/sources/images/button/play.png differ diff --git a/librviz_tutorial/sources/images/black.png b/point_visual/sources/images/color/black.png similarity index 100% rename from librviz_tutorial/sources/images/black.png rename to point_visual/sources/images/color/black.png diff --git a/librviz_tutorial/sources/images/blue.png b/point_visual/sources/images/color/blue.png similarity index 100% rename from librviz_tutorial/sources/images/blue.png rename to point_visual/sources/images/color/blue.png diff --git a/librviz_tutorial/sources/images/green.png b/point_visual/sources/images/color/green.png similarity index 100% rename from librviz_tutorial/sources/images/green.png rename to point_visual/sources/images/color/green.png diff --git a/librviz_tutorial/sources/images/lightgreen.png b/point_visual/sources/images/color/lightgreen.png similarity index 100% rename from librviz_tutorial/sources/images/lightgreen.png rename to point_visual/sources/images/color/lightgreen.png diff --git a/librviz_tutorial/sources/images/orange.png b/point_visual/sources/images/color/orange.png similarity index 100% rename from librviz_tutorial/sources/images/orange.png rename to point_visual/sources/images/color/orange.png diff --git a/librviz_tutorial/sources/images/pink.png b/point_visual/sources/images/color/pink.png similarity index 100% rename from librviz_tutorial/sources/images/pink.png rename to point_visual/sources/images/color/pink.png diff --git a/librviz_tutorial/sources/images/purple.png b/point_visual/sources/images/color/purple.png similarity index 100% rename from librviz_tutorial/sources/images/purple.png rename to point_visual/sources/images/color/purple.png diff --git a/librviz_tutorial/sources/images/red.png b/point_visual/sources/images/color/red.png similarity index 100% rename from librviz_tutorial/sources/images/red.png rename to point_visual/sources/images/color/red.png diff --git a/librviz_tutorial/sources/images/skyblue.png b/point_visual/sources/images/color/skyblue.png similarity index 100% rename from librviz_tutorial/sources/images/skyblue.png rename to point_visual/sources/images/color/skyblue.png diff --git a/librviz_tutorial/sources/images/white.png b/point_visual/sources/images/color/white.png similarity index 100% rename from librviz_tutorial/sources/images/white.png rename to point_visual/sources/images/color/white.png diff --git a/librviz_tutorial/sources/images/yellow.png b/point_visual/sources/images/color/yellow.png similarity index 100% rename from librviz_tutorial/sources/images/yellow.png rename to point_visual/sources/images/color/yellow.png diff --git a/librviz_tutorial/sources/user/input.txt b/point_visual/sources/user/input.txt similarity index 100% rename from librviz_tutorial/sources/user/input.txt rename to point_visual/sources/user/input.txt diff --git a/librviz_tutorial/sources/user/user.txt b/point_visual/sources/user/user.txt similarity index 100% rename from librviz_tutorial/sources/user/user.txt rename to point_visual/sources/user/user.txt diff --git a/librviz_tutorial/src/doc/conf.py b/point_visual/src/doc/conf.py similarity index 100% rename from librviz_tutorial/src/doc/conf.py rename to point_visual/src/doc/conf.py diff --git a/librviz_tutorial/src/doc/index.rst b/point_visual/src/doc/index.rst similarity index 100% rename from librviz_tutorial/src/doc/index.rst rename to point_visual/src/doc/index.rst diff --git a/librviz_tutorial/src/doc/myviz.png b/point_visual/src/doc/myviz.png similarity index 100% rename from librviz_tutorial/src/doc/myviz.png rename to point_visual/src/doc/myviz.png diff --git a/librviz_tutorial/src/doc/tutorialformatter.py b/point_visual/src/doc/tutorialformatter.py similarity index 100% rename from librviz_tutorial/src/doc/tutorialformatter.py rename to point_visual/src/doc/tutorialformatter.py diff --git a/librviz_tutorial/src/loginDialog.cpp b/point_visual/src/loginDialog.cpp similarity index 71% rename from librviz_tutorial/src/loginDialog.cpp rename to point_visual/src/loginDialog.cpp index 43264e4..1e50af6 100644 --- a/librviz_tutorial/src/loginDialog.cpp +++ b/point_visual/src/loginDialog.cpp @@ -9,8 +9,10 @@ LoginDialog::LoginDialog(QWidget *parent):QDialog(parent) { + + this->setFixedSize(380, 300); userLabel = new QLabel(this); - userLabel->move(80, 80); + userLabel->move(70, 80); userLabel->setText(tr("用户名")); userEditLine = new QLineEdit(this); @@ -18,7 +20,7 @@ LoginDialog::LoginDialog(QWidget *parent):QDialog(parent) userEditLine->setPlaceholderText(tr("请输入用户名")); pwdLabel = new QLabel(this); - pwdLabel->move(80, 130); + pwdLabel->move(70, 130); pwdLabel->setText(tr("密码")); pwdEditLine = new QLineEdit(this); @@ -33,23 +35,24 @@ LoginDialog::LoginDialog(QWidget *parent):QDialog(parent) exitBtn = new QPushButton(this); exitBtn->move(220, 200); exitBtn->setText(tr("退出")); - + connect(loginBtn, &QPushButton::clicked, this, &LoginDialog::login); connect(exitBtn, &QPushButton::clicked, this, &LoginDialog::close); // 待解锁的组件名称 - setting_name = {"laser_topic", "open_file", "bag_progress"}; + setting_name = {"cloud_topic", "laser_topic", "open_file", "bag_progress", "fixd_frame_topic"}; + // 账号路径 + user_file_name = "../PointVisual/src/point_visual/sources/user/user.txt"; - // to do read from file + // 创建用户表存下 createUser(); // userPasswords["root"] = hashPassword("root123"); // userPasswords["wbw"] = hashPassword("123"); } - void LoginDialog::createUser(){ - std::ifstream inputFile("../Rviz/librviz_ws/src/librviz_tutorial/sources/user/user.txt"); + std::ifstream inputFile(user_file_name); // 第一次读写user.txt将user加密后，建立userPasswords if(inputFile.is_open()){ std::string line; @@ -57,7 +60,7 @@ void LoginDialog::createUser(){ std::istringstream iss(line); std::string username, pwd; if(iss>> username && iss>> pwd){ - if(pwd.size() < 8){ + if(pwd.size() <= 8){ // 根据长度判断是否已经被加密 pwd = hashPassword(pwd); } @@ -74,7 +77,7 @@ void LoginDialog::createUser(){ // 加密后的账户密码写入user.txt void LoginDialog::writeHash(const std::map user){ - std::ofstream file("../Rviz/librviz_ws/src/librviz_tutorial/sources/user/user.txt"); + std::ofstream file(user_file_name); if(file.is_open()){ for(auto iter = user.begin(); iter != user.end() ; iter++){ @@ -124,15 +127,16 @@ std::string LoginDialog::hashPassword(const std::string& password) { // 判断用户账号返回不同code int LoginDialog::checkUser(const QString username, const QString pwd){ auto iter = userPasswords.find(username); - if (iter != userPasswords.end()) { - if(iter->second == hashPassword(pwd)){ - // 用户密码都对 - return 2; - }else{ - // 用户对，密码不对 - return 1; - } - } else { + if(iter != userPasswords.end()){ + if(pwd.size() > 8) { return 1; } + if(iter->second == hashPassword(pwd)){ + // 用户密码都对 + return 2; + }else{ + // 用户对，密码不对 + return 1; + } + }else { // 用户不对 return 0; } @@ -168,35 +172,6 @@ void LoginDialog::login() userEditLine->clear(); pwdEditLine->clear(); userEditLine->setFocus(); - // //判断用户名和密码是否正确 todo 函数判断 - // if (userEditLine->text().trimmed() == tr("tom") && - // pwdEditLine->text() == tr("123456")) - // { - // for(int i=0 ; ifindChild(setting_name[i]); - // if (foundChild) { - // qDebug()<<"111" ; - // // 找到了指定的子组件 在这里可以对子组件进行操作 - // foundChild->setEnabled(true); - // }else{ - // qDebug()<<"第" << i+1 <<"组件未找到" ; - // } - // } - // QMessageBox::information(this, tr("登录成功"), tr("已解锁"), QMessageBox::Yes); - // // landed = 1; - // accept(); - // } - // else - // { - // QMessageBox::warning(this, tr("登录失败"), tr("用户名或者密码错误"), QMessageBox::Yes); - // } - // userEditLine->clear(); - // pwdEditLine->clear(); - // userEditLine->setFocus(); - - - // QMessageBox::information(this, tr("提示"), tr("已经登陆，请勿重新登录"), QMessageBox::Yes); - } diff --git a/librviz_tutorial/src/loginDialog.h b/point_visual/src/loginDialog.h similarity index 97% rename from librviz_tutorial/src/loginDialog.h rename to point_visual/src/loginDialog.h index aed63ae..fa60a2a 100644 --- a/librviz_tutorial/src/loginDialog.h +++ b/point_visual/src/loginDialog.h @@ -46,6 +46,7 @@ private: // std::vector setting_name; // 用户表 std::map userPasswords; + std::string user_file_name; }; diff --git a/librviz_tutorial/src/main.cpp b/point_visual/src/main.cpp similarity index 100% rename from librviz_tutorial/src/main.cpp rename to point_visual/src/main.cpp diff --git a/point_visual/src/myviz.cpp b/point_visual/src/myviz.cpp new file mode 100644 index 0000000..2e8ef19 --- /dev/null +++ b/point_visual/src/myviz.cpp @@ -0,0 +1,2038 @@ +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rviz/properties/property_tree_widget.h" +#include "rviz/selection/selection_manager.h" +// #include "rviz/views_panel.h" +#include "rviz/panel.h" +#include "rviz/view_controller.h" +#include "rviz/view_manager.h" +#include "rviz/view_controller.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) + : QWidget( parent ) +{ + + nh=ros::NodeHandle("~"); + sub=nh.subscribe("/test",1000,&MyViz::subCallback,this); + pub=nh.advertise("/test_pub",1000); + pub_thread=std::thread(&MyViz::pubThread,this); + + // 全局变量存放处 + // 颜色名称，可添加，待设置rgb类型设置颜色 + circle_index = 0; + color_name = { + "white","red","green","black","blue","yellow", + "pink","purple","skyblue","lightgreen","orange", + }; + // 颜色路径名 + color_icon_filename = QString("../PointVisual/src/point_visual/sources/images/color/%1.png"); + button_icon_filename = QString("../PointVisual/src/point_visual/sources/images/button/%1.png"); + speed_name = {"2.0","1.5","1.25","1.0","0.75","0.5",}; + // circle_radius = {}; + ispauseclick = true; + sliderreleased = false; + playagain = false; + isthreadplaying = false; + stored = false; + + dlg = new LoginDialog(this); + landed = 0; + cur_fixed_frame = "world"; + + // 网格结构 + QGridLayout* controls_layout = new QGridLayout(); + int ctl_index = 0; + + // 登录按钮 + QPushButton* login_button = new QPushButton("登录"); + QPushButton* quit_login_button = new QPushButton("退出"); + + //select按钮 + QPushButton* selection_button = new QPushButton("选择坐标点"); + QPushButton* move_camera_button = new QPushButton("移动视角"); + + controls_layout->addWidget( login_button, ctl_index, 0); + controls_layout->addWidget( quit_login_button, ctl_index++, 1); + + // offline 布局 + // QPushButton setting!//////////////////////////////////////////////////////////////////////////////////////////////// + QIcon pause_png(button_icon_filename.arg("pause")); + QIcon gogo_png(button_icon_filename.arg("approach")); + QIcon back_png(button_icon_filename.arg("back")); + QIcon play_png(button_icon_filename.arg("play")); + + play = new QPushButton(play_png, "", this); + + QPushButton* but_save = new QPushButton("保存一帧", this);// buttonn: save one frame + QPushButton* but_saveMul = new QPushButton("保存多帧", this);// button: save multiple frames + QPushButton* but_file = new QPushButton("打开文件", this);// button: select file + + QPushButton* but_nextFrame = new QPushButton(gogo_png,"", this);// button: next frame********************************************************************************************************************* + QPushButton* but_previousFrame = new QPushButton(back_png,"", this);// button: previous frame********************************************************************************************************************* + + // QLabel setting!///////////////////////////////////////////////////////////////////////////////////////////////////// + QLabel* label_speed = new QLabel("播放速度:");//speed combobox + QLabel* label_split = new QLabel("");//speed combobox + + // QSlider setting!//////////////////////////////////////////////////////////////////////////////////////////////////// + progress = new QSlider(Qt::Horizontal); + // QComboBox setting!////////////////////////////////////////////////////////////////////////////////////////////////// + QComboBox * speed_combobox = new QComboBox(); + + QPushButton* but_trans = new QPushButton("格式转换", this);// buttonn: save one frame + + //enable button//////////////////////////////////////////////////////////////////////////////////////////////////////// + play->setEnabled(true); + but_save->setEnabled(true); + but_saveMul->setEnabled(true); + but_file->setEnabled(false); + + // setting button size!//////////////////////////////////////////////////////////////////////////////////////////////// + play->setFixedSize(130, 30); + but_save->setFixedSize(130, 30); + but_saveMul->setFixedSize(130, 30); + but_file->setFixedSize(130, 30); + + but_nextFrame->setFixedSize(130, 30); + but_previousFrame->setFixedSize(130, 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_trans, ctl_index++,1); + + controls_layout->addWidget( but_save, ctl_index, 0 ); + controls_layout->addWidget( but_saveMul, ctl_index++, 1 ); + controls_layout->addWidget( move_camera_button, ctl_index, 0); + controls_layout->addWidget( selection_button, ctl_index++ ,1); + + 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( but_previousFrame, ctl_index, 0 ); + controls_layout->addWidget( but_nextFrame, 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("参考坐标系:"); + QCheckBox *axes_checkbox = new QCheckBox("", this); + axes_checkbox->setStyleSheet("QCheckBox::indicator { width: 20px; height: 20px; }"); + // QComboBox * axesBox = new QComboBox(); + // axesBox->addItem("不显示"); + // axesBox->addItem("显示"); + + // 选择圆圈范围 + QLabel* label_circle = new QLabel("距离参考圈(m): "); + QLineEdit* circleLineEdit = new QLineEdit(); + circleLineEdit->setPlaceholderText("请输入数字"); + circleLineEdit->setFixedSize(130,30); + // 设置父组件 + circleLineEdit->setObjectName("circle_edit"); + circleLineEdit->setParent(this); + // 增加圆圈增加，删除按钮 + QPushButton* but_addCircle = new QPushButton("添加参考圈"); + QPushButton* but_deleteCircle = new QPushButton("删除参考圈"); + + // 以红色坐标轴指向自己为正视图 + QPushButton* but_view0 = new QPushButton("正视图"); + QPushButton* but_view_left = new QPushButton("左视图"); + QPushButton* but_view_right = new QPushButton("右视图"); + QPushButton* but_view_top = new QPushButton("俯视图"); + + + controls_layout->addWidget(but_addCircle, ctl_index, 0); + controls_layout->addWidget(but_deleteCircle, ctl_index++, 1); + + controls_layout->addWidget(label_circle, ctl_index,0); + controls_layout->addWidget(circleLineEdit, ctl_index++,1); + + controls_layout->addWidget(label_axes, ctl_index,0); + controls_layout->addWidget(axes_checkbox, ctl_index++,1); + + + controls_layout->addWidget(but_view0, ctl_index, 0); + controls_layout->addWidget(but_view_top, ctl_index++, 1); + controls_layout->addWidget(but_view_left, ctl_index, 0); + controls_layout->addWidget(but_view_right, ctl_index++, 1); + + // Tree 全局变量存放地方 + Tree_Display(controls_layout, ctl_index++); + + // 待解锁子组件设置父组件，方便寻找，登陆后解锁可编辑属性 + login_button->setObjectName("login_button"); + login_button->setParent(this); + quit_login_button->setObjectName("quit_login_button"); + quit_login_button->setParent(this); + quit_login_button->setEnabled(false); + + but_file->setObjectName("open_file"); + but_file->setParent(this); + progress->setObjectName("bag_progress"); + progress->setParent(this); + progress->setEnabled(false); + + // controls_layout->setRowStretch(3, 0); + // Make signal/slot connections. 进行信号/插槽连接 + + // 增加登录按钮 + connect(login_button, SIGNAL(clicked()), this, SLOT(login_button_clicked())); + // 增加退出按钮 + connect(quit_login_button, SIGNAL(clicked()), this, SLOT(quit_login_button_clicked())); + + // 增加切换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(axes_checkbox, SIGNAL(stateChanged(int)), this, SLOT(AxesDisplayChanged(int))); + // 增加圆圈变换-弃用 + // connect(circleLineEdit, SIGNAL(textChanged(const QString&)), this, SLOT(CircleDisplayChanged(const QString&))); + // 增加圆圈 + connect(but_addCircle, SIGNAL(clicked()), this, SLOT(CircleDisplayAdded())); + // 删除圆圈 + connect(but_deleteCircle, SIGNAL(clicked()), this, SLOT(CircleDisplayDeleted())); + // 增加视角回正 + connect(but_view0, SIGNAL(clicked()), this, SLOT(ViewZero())); + // 增加视角左转 + connect(but_view_left, SIGNAL(clicked()), this, SLOT(ViewLeft())); + // 增加视角右转 + connect(but_view_right, SIGNAL(clicked()), this, SLOT(ViewRight())); + // 增加视角俯视 + connect(but_view_top, SIGNAL(clicked()), this, SLOT(ViewTop())); + + //off + connect(but_nextFrame, &QPushButton::clicked, this, [&]() { + if(!ispauseclick){ + is_next = true; + }else{ + QMessageBox::information(this, "tips", "No file is played"); + } + }); + connect(but_previousFrame, &QPushButton::clicked, this, [&]() { + if(!ispauseclick){ + is_previous = true; + }else{ + QMessageBox::information(this, "tips", "No file is played"); + } + }); + + connect(progress,&QSlider::valueChanged,this,[=](){// 变动会触发 + if(!ismoved){ + ischanged = true;// “戳”这一个动作会使ischange变成true + } + }); + connect(progress,&QSlider::sliderMoved,this,[=](){// 变动会触发 + sliderreleased = true; + ismoved = true; + }); + connect(progress,&QSlider::sliderPressed,this,[=](){// 拖动时打开暂停 + ispauseclick = false; + // play->setText("Play"); + play->setIcon(QIcon(play_png)); + }); + connect(progress,&QSlider::sliderReleased,this,[=](){// 释放时关闭暂停 + // sliderreleased = true; + ispauseclick = false; + ismoved = false; + // play->setText("Play"); + play->setIcon(QIcon(play_png)); + }); + + connect(but_save, &QPushButton::clicked, this, [&]() { + if(isthreadplaying){ + ispauseclick = false; + SavefileName = QFileDialog::getSaveFileName(nullptr, "Save File", "/home","files(*)"); + is_save = true; + } + // QMessageBox::information(this, "tips", "Save Complete!");} + 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; + QMessageBox::information(this, "tips", "Save Mul Complete!"); + } + else{ + SaveMulfileName = QFileDialog::getSaveFileName(nullptr, "Save File", "/home","files(*)"); + is_saveMul = true; + std::string nametype = find(SaveMulfileName.toStdString()); + if(!ispauseclick && !is_saveMulFinish && is_saveMul && nametype=="bag") + { + QMessageBox::information(this, "tips", "Move Slider to Choose End!"); + } + } + }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(threadout && !playagain){ + threadout = false; + playagain = true; + ispauseclick = true; + isthreadplaying = true; + stored = true; + // play->setText("Pause"); + play->setIcon(pause_png); + std::string choice = find(filePath.toStdString()); + int filename; + if(choice == "bag") + { + filename = 0; + if(Topic.isEmpty()) + { + QMessageBox msgBox; + msgBox.setWindowTitle("Tips"); + msgBox.setText("Please set topic!"); + msgBox.resize(2000, 1000); + msgBox.exec(); + filename = -1; + isthreadplaying = false; + playagain = false; + threadout = true; + ispauseclick = false; + stored = false; + } + } + if(choice == "pcd"){filename = 1;} + if(choice == "ply"){filename = 2;} + if(choice == "bin"){filename = 3;} + + switch(filename) + { + case 0: + {std::thread rosBagPlay(&MyViz::openFile, this); + rosBagPlay.detach(); + break;} + case 1: + {std::thread rosBagPlay(&MyViz::pcdFile, this); + rosBagPlay.detach(); + break;} + case 2: + {std::thread rosBagPlay(&MyViz::plyFile, this); + rosBagPlay.detach(); + break;} + case 3: + {std::thread rosBagPlay(&MyViz::bin, this); + rosBagPlay.detach(); + break;} + } + } + else if(isthreadplaying && playagain && ispauseclick){ // pause + ispauseclick = false; + play->setIcon(play_png); + }else if(isthreadplaying && playagain && ! ispauseclick){ // not pause + ispauseclick = true;; + play->setIcon(pause_png); + } + + + } + + }); + // connect(bg_topic_text,SIGNAL(textChanged(const QString&)),this,SLOT(setTopic(const QString&))); + + connect(but_trans, &QPushButton::clicked, this, &MyViz::TransformeType); + + // Construct and lay out render panel. + render_panel_ = new rviz::RenderPanel(); // RVIZ在QT上显示的类 + QHBoxLayout* main_layout = new QHBoxLayout; // 主要layout QV垂直，QH水平 + + // Next we initialize the main RViz classes. + // + // The VisualizationManager is the container for Display objects, + // holds the main Ogre scene, holds the ViewController, etc. It is + // very central and we will probably need one in every usage of + // librviz. + manager_ = new rviz::VisualizationManager( render_panel_ ); // 是实现rviz功能的类 + // 初始化camera 这行代码实现放大 缩小 平移等操作 + render_panel_->initialize( manager_->getSceneManager(), manager_ ); + manager_->setFixedFrame("laser"); + // 初始化tool_manager_ + tool_manager_ = manager_->getToolManager(); + + // 在初始化之后再进行更改操作 + manager_->initialize(); + manager_->startUpdate(); + + manager_->setFixedFrame(cur_fixed_frame); + + // 创建select panel + QLabel *select_label = new QLabel("坐标点显示:"); + select_tree_widget_ = new rviz::PropertyTreeWidget(); + select_tree_widget_->setModel(manager_->getSelectionManager()->getPropertyModel()); + ctl_index++; + controls_layout->addWidget(select_label, ctl_index++, 0); + controls_layout->addWidget(select_tree_widget_, ctl_index++, 0, 1, 2); + + // view_tree_widget_ = new rviz::PropertyTreeWidget(); + // 视角类ViewManager + view_man_ = manager_->getViewManager(); + viewController = view_man_ ->getCurrent(); + ROS_ASSERT( viewController != NULL ); + // view_tree_widget_->setModel(view_man_ -> getPropertyModel()); + // controls_layout->addWidget(view_tree_widget_, ctl_index++, 0, 1, 2); + + // 设置网格某一行伸缩比例,放最后一行 + // controls_layout->setRowStretch(--ctl_index, 6); + + // Create a Grid display. + grid_ = manager_->createDisplay( "rviz/Grid", "adjustable grid", true ); + ROS_ASSERT( grid_ != NULL ); + grid_->subProp( "Line Style" )->setValue( "Billboards" ); + grid_->subProp( "Color" )->setValue( QColor( Qt::white ) ); + + // 新增点云 + cloud = manager_->createDisplay( "rviz/PointCloud2", "point cloud", true ); + ROS_ASSERT( cloud != NULL ); + cloud->subProp("Size (m)")->setValue("0.005"); + cloud->subProp("Color Transformer")->setValue("FlatColor"); + cloud->subProp("Style")->setValue("Flat Squares"); + // cloud->subProp("Topic")->setValue("/pose_graph/octree"); + // cloud->subProp("Invert Rainbow")->setValue("true"); + // cloud->subProp("Decay Time")->setValue("1"); + // Configure the GridDisplay the way we like it. + + laser_ = manager_->createDisplay( "rviz/LaserScan", "Qlaser", true ); + ROS_ASSERT( laser_ ); + // 新增雷达 + laser_->subProp("Topic")->setValue("/scan"); + laser_->subProp("Size (m)")->setValue("0.015"); + laser_->subProp("Color Transformer")->setValue("FlatColor"); + laser_->subProp("Color")->setValue(color_name[1]); + + // 新增坐标系 + axes_display= manager_->createDisplay("rviz/Axes", "Axes", false); + ROS_ASSERT( axes_display != NULL ); + axes_display->subProp("Length")->setValue(0.2); // 设置透明度 + axes_display->subProp("Radius")->setValue(0.05); // 设置坐标系的缩放大小 + // 新增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( controls_layout ); + // main_layout->addWidget( view_tree_widget_); + main_layout->addWidget( render_panel_,2); // 存放rviz的位置, stretch 拉伸系数 + setLayout( main_layout ); + +} + +void MyViz::Tree_Display(QGridLayout* controls_layout, int index){ + QTreeWidget *menu = new QTreeWidget(); + // menu->setColumnCount(2); + // 列头自适应大小 + menu->header()->setSectionResizeMode(QHeaderView::ResizeToContents); + menu->setHeaderLabels(QStringList()<<"key"<<"value"); + menu->setHeaderHidden(true); + + // 设置不同层次菜单的缩进 + // menu->setIndentation(10); + // global-一层树 + QTreeWidgetItem *global= new QTreeWidgetItem(menu, QStringList("全局变量")); + menu->addTopLevelItem(global); + global->setExpanded(true); + + // fix frame + QTreeWidgetItem *fixed_frame = new QTreeWidgetItem(global, QStringList("固定参考系")); + QLineEdit* fixed_frame_text = new QLineEdit(); + fixed_frame_text->setText("world"); + // 方便寻找，登陆后解锁 + fixed_frame_text->setObjectName("fixd_frame_topic"); + fixed_frame_text->setParent(this); + fixed_frame_text->setEnabled(false); + // fixed_frame_text->setStyleSheet("background:transparent;border-width:0;border-style:inset"); + // 初始化固定坐标系 + cur_fixed_frame = fixed_frame_text->text(); + // 设置事件 + 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("背景颜色 ")); + QLineEdit* bg_color_text = new QLineEdit(); + bg_color_text->setText("50;50;50"); + // bg_color_text->setFixedWidth(130); + // 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); + // global->setTextAlignment(1, 2); // (列数，对齐方式) + // grid - 一层树 + QTreeWidgetItem * grid = new QTreeWidgetItem(menu, QStringList("栅格")); + // 二层 + QTreeWidgetItem *grid_color = new QTreeWidgetItem(grid, QStringList("颜色")); + QComboBox * grid_color_cbox = 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); + 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); + + // grid cell size + QTreeWidgetItem *grid_cell = new QTreeWidgetItem(grid, QStringList("网格大小")); + QSlider* cell_size_slider = new QSlider( Qt::Horizontal ); + cell_size_slider->setMinimum( 1 ); + cell_size_slider->setMaximum( 100 ); + cell_size_slider->setValue(10); + connect( cell_size_slider, SIGNAL( valueChanged( int )), this, SLOT( setCellSize( int ))); + menu->setItemWidget(grid_cell, 1, cell_size_slider); + + // grid Line Thickness + QTreeWidgetItem *grid_thickness = new QTreeWidgetItem(grid, QStringList("线条粗细")); + QSlider* thickness_slider = new QSlider( Qt::Horizontal ); + thickness_slider->setMinimum( 1 ); + thickness_slider->setMaximum( 100 ); + thickness_slider->setValue(5); + connect( thickness_slider, SIGNAL( valueChanged( int )), this, SLOT( setThickness( int ))); + + menu->setItemWidget(grid_thickness, 1, thickness_slider); + + // laser - 一层树 + QTreeWidgetItem * laser = new QTreeWidgetItem(menu, QStringList("雷达")); + // 二层 + QTreeWidgetItem *laser_topic = new QTreeWidgetItem(laser, QStringList("话题")); + // Topic_Laser 输入雷达 + QLineEdit* laser_topic_text = new QLineEdit(); + + // 方便寻找，登陆后解锁 + laser_topic_text->setObjectName("laser_topic"); + laser_topic_text->setParent(this); + laser_topic_text->setEnabled(false); + // 增加雷达话题 + 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("大小(m)")); + QLineEdit* laser_size_text=new QLineEdit(); + laser_size_text->setText("0.015"); + connect(laser_size_text,SIGNAL(textChanged(const QString &)),this,SLOT(setLaserSize(const QString &))); + menu->setItemWidget(laser_size, 1, laser_size_text); + + // Laser Alpha + QTreeWidgetItem *laser_alpha = new QTreeWidgetItem(laser, QStringList("透明度")); + QLineEdit* laser_alpha_text=new QLineEdit(); + laser_alpha_text->setText("1"); + connect(laser_alpha_text,SIGNAL(textChanged(const QString &)),this,SLOT(setLaserAlpha(const QString &))); + menu->setItemWidget(laser_alpha, 1, laser_alpha_text); + + // Laser color + QTreeWidgetItem *laser_color = new QTreeWidgetItem(laser, QStringList("颜色")); + QComboBox * laser_color_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); + laser_color_comboBox->addItem(color_icon, color_name[i]); + } + // 设置初始化雷达颜色为红色 + laser_color_comboBox->setCurrentIndex(1); + // 增加颜色变换 + connect(laser_color_comboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(LaserColorChanged(int))); + menu->setItemWidget(laser_color, 1, laser_color_comboBox); + + // cloud - 一层树 + QTreeWidgetItem * cloud = new QTreeWidgetItem(menu, QStringList("点云")); + // 二层 + QTreeWidgetItem *cloud_topic = new QTreeWidgetItem(cloud, QStringList("话题")); + // Colud_Topic 输入雷达 + QLineEdit* cloud_topic_text = new QLineEdit(); + cloud_topic_text->setObjectName("cloud_topic"); + cloud_topic_text->setParent(this); + cloud_topic_text->setEnabled(false); + connect(cloud_topic_text, SIGNAL(textChanged(const QString &)), this, SLOT(setCloudTopic(const QString &))); + menu->setItemWidget(cloud_topic, 1, cloud_topic_text); + + // Cloud Size + QTreeWidgetItem *cloud_size = new QTreeWidgetItem(cloud, QStringList("大小(m)")); + QLineEdit* cloud_size_text=new QLineEdit(); + cloud_size_text->setText("0.005"); + connect(cloud_size_text,SIGNAL(textChanged(const QString &)),this,SLOT(setCloudSize(const QString &))); + menu->setItemWidget(cloud_size, 1, cloud_size_text); + + // Cloud Alpha + QTreeWidgetItem *cloud_alpha = new QTreeWidgetItem(cloud, QStringList("透明度")); + QLineEdit* cloud_alpha_text=new QLineEdit(); + cloud_alpha_text->setText("1"); + connect(cloud_alpha_text,SIGNAL(textChanged(const QString &)),this,SLOT(setCloudAlpha(const QString &))); + menu->setItemWidget(cloud_alpha, 1, cloud_alpha_text); + + // cloud color + QTreeWidgetItem *cloud_color = new QTreeWidgetItem(cloud, QStringList("颜色")); + QComboBox * cloud_color_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); + cloud_color_comboBox->addItem(color_icon, color_name[i]); + } + // 设置初始化颜色 + cloud_color_comboBox->setCurrentIndex(0); + + connect(cloud_color_comboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(CloudColorChanged(int))); + menu->setItemWidget(cloud_color, 1, cloud_color_comboBox); + + //controls_layout->addWidget(menu, index, 0 ); + controls_layout->addWidget(menu, index, 0, 1, 2); +} + +void MyViz::ViewTop(){ + viewController->reset(); + viewController->subProp("Yaw")->setValue(0); + viewController->subProp("Pitch")->setValue(1.5698); +} + +void MyViz::ViewLeft(){ + viewController->reset(); + viewController->subProp("Yaw")->setValue(1.570796*3); + viewController->subProp("Pitch")->setValue(0); +} + +void MyViz::ViewRight(){ + viewController->reset(); + // 0.785398是以红色为正的右边45度 + viewController->subProp("Yaw")->setValue(1.570796); + viewController->subProp("Pitch")->setValue(0); +} + +void MyViz::ViewZero(){ + if (viewController) + { + viewController->reset(); + viewController->subProp("Yaw")->setValue(0); + viewController->subProp("Pitch")->setValue(0); + } +} + +void MyViz::quit_login_button_clicked(){ + landed = 0; + // 退出设置登录按钮可用 + QWidget * login_button = this->findChild("login_button"); + if (login_button) { + login_button->setEnabled(true); + }else{ + qDebug() << "login_button组件未找到" ; + } + // 退出设置退出按钮不可用 + QWidget * quit_button = this->findChild("quit_login_button"); + if (quit_button) { + quit_button->setEnabled(false); + }else{ + qDebug() << "quit_login_button组件未找到" ; + } + // 退出设置其他组件不可用 + if(dlg != NULL){ + for (int i=0; i< dlg->setting_name.size(); i++){ + QWidget *foundChild = this->findChild(dlg->setting_name[i]); + if (foundChild) { + // 找到了指定的子组件 在这里可以对子组件进行操作 + foundChild->setEnabled(false); + }else{ + qDebug()<<"第" << i+1 <<"组件未找到" ; + } + } + } +} + +void MyViz::login_button_clicked(){ + dlg->setWindowTitle(tr("登录")); + // dlg->open();// 非模态级别对话框 + if(dlg->exec() == QDialog::Accepted){ + // 登录成功后 + landed = 1; + qDebug() << "login accepted"; + // 登录设置其他组件和退出可用 + QWidget * quit_button = this->findChild("quit_login_button"); + if (quit_button) { + quit_button->setEnabled(true); + }else{ + qDebug() << "quit_login_button组件未找到" ; + } + // 登录设置登录不可用 + QWidget * login_button = this->findChild("login_button"); + if (login_button) { + login_button->setEnabled(false); + }else{ + qDebug()<<"login_button组件未找到" ; + } + }else{ + // 登录失败 + qDebug() << "login refused"; + } +} + +void MyViz::PlayCircles(){ + int argc =0; + char** argv; + ros::init(argc, argv, "basic_shapes"); + ros::NodeHandle n; + ros::Rate r(100); + ros::Publisher marker_pub = n.advertise("visualization_marker", 10); + uint32_t shape = visualization_msgs::Marker::LINE_STRIP; + double radius = 0; // 圆的半径 + + if(ros::ok()) + { + for(auto it = circle_radius.begin(); it != circle_radius.end(); ++it){ + // qDebug() << it->first <<"r_test_playcicle"; + visualization_msgs::Marker marker; + // Set the frame ID and timestamp. See the TF tutorials for information on these. + marker.header.frame_id = cur_fixed_frame.toStdString(); + // 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 = it->second; + // 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]); + int num_points = 360; + + // Create the vertices for the points and lines 画圆 + radius = it->first; + for (uint32_t i = 0; i < num_points; ++i) + { + 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) // 测试rviz点击就更新了，全部更新 + { + 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::CircleDisplayDeleted(){ + if(circle_radius.empty()){ + // 如果为空，置index为0，也可以不置 + circle_index = 0; + } + QLineEdit * circle_edit = this->findChild("circle_edit"); + if (circle_edit && !circle_radius.empty()) { + QString text = circle_edit->text(); + double r = text.toDouble(); + // 从map中找到半径元素 + auto it = circle_radius.find(r); + // 找到半径 + if( it != circle_radius.end()){ + // 更新界面 + marker_->reset(); + // 先删除再发布 + circle_radius.erase(it); + std::thread ThreadCircleDelete(&MyViz::PlayCircles, this); + ThreadCircleDelete.detach(); + // qDebug() << "删除成功" << it->first ; + } + }else{ + // to do + // qDebug() << "circle_edit组件未找到 或 circle_radius为空" ; + } +} + +// 增加圆圈槽函数 +void MyViz::CircleDisplayAdded(){ + marker_->setValue(true); + QLineEdit * circle_edit = this->findChild("circle_edit"); + if (circle_edit) { + // 获取要删除的半径 + QString text = circle_edit->text(); + double r = text.toDouble(); + if( r == 0 ){ + // qDebug() << "输入半径为0"; + // marker_->setValue(false); + // to do + }else{ + // marker_->setValue(true); + // 判断半径重复 + if(circle_radius.find(r) == circle_radius.end()){ + // 未找到相同的半径，则添加 + circle_radius[r] = circle_index; + circle_index++; + }else{ + qDebug() << "相同元素" << r <<"已添加"; + } + marker_->reset(); + std::thread ThreadCircle(&MyViz::PlayCircles, this); + ThreadCircle.detach();// 执行完成后自动回收资源 + } + }else{ + qDebug() << "circle_edit组件未找到" ; + } +} + +// 参考系变化槽函数 +void MyViz::AxesDisplayChanged(int stage) +{ + // 0未被选中，1被部分选中，2 被选中 + switch (stage) + { + case 0: // 未被选中 + if (axes_display != NULL) + axes_display->setValue(false); + break; + + case 2: // 被选择 + 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; + QColor new_color = parseColor(t); + if(new_color.isValid()){ + // rviz源码中修改背景颜色的方式 + render_panel_->setBackgroundColor(qtToOgre(new_color)); + manager_->queueRender(); + } + else + { + ROS_INFO_STREAM("Color invalid !!!"); + } + }else{ + ROS_INFO_STREAM("manager_ failed !!!"); + } +} + + +void MyViz::LaserColorChanged(int index){ + if(laser_!=NULL) laser_->subProp("Color")->setValue(color_name[index]); + +} + +void MyViz::CloudColorChanged(int index){ + if(cloud!=NULL) cloud->subProp("Color")->setValue(color_name[index]); + +} +void MyViz::GridColorChanged(int index){ + if(grid_!=NULL) grid_->subProp("Color")->setValue(color_name[index]); +} + +// Destructor. +MyViz::~MyViz() +{ + delete manager_; +} + +void MyViz::setThickness( int thickness_percent ) +{ + if( grid_ != NULL ) + { + grid_->subProp( "Line Style" )->subProp( "Line Width" )->setValue( thickness_percent / 100.0f ); + } +} + +void MyViz::setCellSize( int cell_size_percent ) +{ + if( grid_ != NULL ) + { + grid_->subProp( "Cell Size" )->setValue( cell_size_percent / 10.0f ); + } +} + +void MyViz::setCloudTopic(const QString &newTopic){ + if(cloud!=NULL){ + Topic = newTopic; + cloud->subProp( "Topic" )->setValue(newTopic); + ROS_INFO_STREAM("cloud topic changed to => "<startUpdate(); +} + +void MyViz::setCloudSize(const QString & size){ + if(cloud!=NULL){ + cloud->subProp("Size (m)")->setValue(size); + } +} + +void MyViz::setCloudAlpha(const QString& cloudAlpha){ + if(cloud!=NULL){ + cloud->subProp("Alpha")->setValue(cloudAlpha); + } +} +void MyViz::setLaserAlpha(const QString& laserAlpha){ + if(laser_!=NULL){ + laser_->subProp("Alpha")->setValue(laserAlpha); + } +} +void MyViz::setLaserTopic(const QString &newTopic){ + if(laser_!=NULL){ + Topic = newTopic; + laser_->subProp( "Topic" )->setValue(newTopic); + ROS_INFO_STREAM("laser topic changed to => "<setEnabled(true); + // laser->subProp( "Color" )->setValue( QColor( Qt::yellow ) ); + // manager_->setFixedFrame("laser"); + manager_->startUpdate(); +} + +void MyViz::setFixedFrame(const QString & fix_frame){ + manager_->setFixedFrame(fix_frame); + cur_fixed_frame = fix_frame; + manager_->startUpdate(); +} + +void MyViz::setLaserSize(const QString& laserSize){ + if(laser_!=NULL){ + laser_->subProp("Size (m)")->setValue(laserSize); + } +} + +void MyViz::subCallback(const std_msgs::String& msg){ + ROS_INFO_STREAM("receive message!"); +} + +void MyViz::pubThread(){ + while(ros::ok()){ + 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可以解决 + + ros::NodeHandle n; + scan_pub = n.advertise(Topic.toStdString(), 5);// "topic" + scan_pubpcd = n.advertise(Topic.toStdString(), 5); + // sleep time + + bag.open(pcdFilePathStr, rosbag::bagmode::Read); + + rosbag::View view1(bag, rosbag::TopicQuery("/" + Topic.toStdString())); + view = &view1; + std::cout<size()<begin(); it != view->end(); ++it)// 用于预先存储bag包的信息 + { + auto m = *it; + sensor_msgs::LaserScan::ConstPtr input = m.instantiate(); + if(input==nullptr){break;} + indx.push_back(input); + } + rosbag::View view2(bag, rosbag::TopicQuery("/" + Topic.toStdString())); + view_ = &view2; + if(view2.size()==0) + { + isthreadplaying = false; + playagain = false; + threadout = true; + ispauseclick = false; + stored = false; + QMessageBox::information(this, "warning", "Error Topic!"); + return; + } + if(indx.size()==0){ + for (rosbag::View::iterator it = view_->begin(); it != view_->end(); ++it)// 用于预先存储bag包的信息 + { + auto m = *it; + sensor_msgs::PointCloud2::ConstPtr input = m.instantiate(); + if(input != nullptr){ + indxpcd.push_back(input);} + } + pointcloud2 = true; + } + stored = false; + indxsize = indx.size(); + indxpcdsize = indxpcd.size(); + bag.close(); + } + + if(!pointcloud2){ + while (isthreadplaying)// 用于播放存储的bag包的信息 + { + if(threadout) + { + break; + } + double countbagvalue = countbag; + if(countbag>=indxsize) + { + scan_pub.publish(*indx[indxsize-1]); + end = true;// 停在当前位置 + countbag = 0;// 从头开始 + } + // progressmove = false; + + if(is_rateChanged){ + r = ros::Rate(speed_rate); + r.reset(); + is_rateChanged = false; + } + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + if(is_save){ + if(newBag.isOpen()) + newBag.close(); + std::string saveFilePathStr = SavefileName.toStdString(); + + qDebug() << SavefileName; + if(SavefileName.isEmpty()){ + QMessageBox msgBox; + msgBox.setWindowTitle("Tips"); + msgBox.setText("Save failure, No file is selected"); + msgBox.resize(2000, 1000); + msgBox.exec(); + is_save = false; + continue; + } + std::string NameType = find(saveFilePathStr); + if(NameType == "bag"){ + std::ifstream file(saveFilePathStr.c_str()); + if (!file.good()) { + QFile newFile(QString::fromStdString(saveFilePathStr)); + newBag.open(saveFilePathStr, rosbag::bagmode::Write); + newBag.write("/scan", ros::Time::now(), indx[countbag]); + newBag.close(); + }else{ + // file exist + } + } + // else if(NameType == "pcd"){ + // pcl::PointCloud pcl_cloud; + // // pcl::fromROSMsg(*indx[countbag], pcl_cloud); + // populatePointCloudFromLaserScan(*indx[countbag], pcl_cloud); + // // 保存点云数据到PCD文件 + // pcl::io::savePCDFileASCII(saveFilePathStr, pcl_cloud); + // } + // }else if(NameType == "ply"){ + // pcl::PointCloud pcl_cloud; + // pcl::fromROSMsg(*indx[countbag], pcl_cloud); + + // // 保存点云数据到PLY文件 + // pcl::io::savePLYFileASCII(saveFilePathStr, pcl_cloud); + // }else if(NameType == "bin"){ + // pcl::PointCloud pcl_cloud; + // pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + + // pcl::fromROSMsg(*indx[countbag], *cloud); + // std::ofstream bin_file(saveFilePathStr.c_str(),std::ios::out|std::ios::binary|std::ios::app); + // if(!bin_file.good()) std::cout<<"Couldn't open "<points.size (); ++i) + // { + // bin_file.write(reinterpret_cast(&cloud->points[i].x), sizeof(float)); + // bin_file.write(reinterpret_cast(&cloud->points[i].y), sizeof(float)); + // bin_file.write(reinterpret_cast(&cloud->points[i].z), sizeof(float)); + // } + + // bin_file.close(); + // } + else{ + QMessageBox msgBox; + msgBox.setWindowTitle("Tips"); + msgBox.setText("file type error"); + msgBox.resize(2000, 1000); + msgBox.exec(); + is_save = false; + } + 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; + } + // QMessageBox::information(this, "tips", "Move Slider to Choose End!"); + if(is_saveMulFinish){ + frameEnd = countbag; + + if(frameStart > frameEnd){ + int tem = frameEnd; + frameEnd = frameStart; + frameStart = tem; + } + std::ifstream file(SaveMulfileName.toStdString().c_str()); + if (!file.good()) { + newBag.open(SaveMulfileName.toStdString(), 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{ + if(!saveFlag){ + std::string NameType = find(SaveMulfileName.toStdString()); + if(SaveMulfileName.isEmpty()) + { + QMessageBox msgBox; + msgBox.setWindowTitle("Tips"); + msgBox.setText("Save failure, No file is selected, Please seve again!"); + msgBox.resize(2000, 1000); + msgBox.exec(); + is_saveMul = false; + continue; + }else if(NameType != "bag"){ + QMessageBox msgBox; + msgBox.setWindowTitle("Tips"); + msgBox.setText("file type error"); + msgBox.resize(2000, 1000); + msgBox.exec(); + is_saveMul = false; + continue; + } + frameStart = countbag; + saveFlag = true; + }else{ + ; + } + } + if(ismoved) + { + double valuedouble = progress->value(); + countbag = round((valuedouble/100)*indxsize); + scan_pub.publish(*indx[countbag]); + progress->setSliderPosition((countbag/indxsize*100)); + ismoved = false; + } + else{ + scan_pub.publish(*indx[countbag]); + progress->setSliderPosition((countbag/indxsize*100)); + r.sleep(); + ischanged = false;} + //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) + { + if(is_next){ + if(countbag == indxsize) + countbag = 0; + else + countbag++; + scan_pub.publish(*indx[countbag]); + progress->setSliderPosition(ceil(countbag/indxsize*100)); + r.sleep(); + ischanged = false; + is_next = false; + // continue; + } + if(is_previous){ + if(countbag > 0) + countbag--; + scan_pub.publish(*indx[countbag]); + progress->setSliderPosition(ceil(countbag/indxsize*100)); + r.sleep(); + ischanged = false; + is_previous = false; + // continue; + } + if(countbagsliderPosition()<=100 && countbagvalue/indxsize*100<=100) + { + progress->setSliderPosition(ceil(countbagvalue/indxsize*100)); + } + countbag++; + ischanged = false; + r.sleep(); + } + } + } + else if(pointcloud2){ + while (isthreadplaying){// 用于播放存储的bag包的信息 + if(threadout) + break; + double countbagvalue = countbag; + if(countbag>=indxpcdsize) + { + scan_pubpcd.publish(*indxpcd[indxpcdsize-1]); + end = true;// 停在当前位置 + countbag = 0;// 从头开始 + } + // progressmove = false; + + if(is_rateChanged){ + r = ros::Rate(speed_rate); + r.reset(); + is_rateChanged = false; + } + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + if(is_save){ + if(newBag.isOpen()) + newBag.close(); + std::string saveFilePathStr = SavefileName.toStdString(); + + qDebug() << SavefileName; + if(SavefileName.isEmpty()){ + QMessageBox msgBox; + msgBox.setWindowTitle("Tips"); + msgBox.setText("Save failure, No file is selected"); + msgBox.resize(2000, 1000); + msgBox.exec(); + is_save = false; + continue; + } + std::string NameType = find(saveFilePathStr); + if(NameType == "bag"){ + 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); + newBag.write("/scan", ros::Time::now(), indxpcd[countbag]); + + newBag.close(); + }else{ + QMessageBox msgBox; + msgBox.setWindowTitle("Tips"); + msgBox.setText("Save failure"); + msgBox.resize(2000, 1000); + msgBox.exec(); + continue; + } + }else if(NameType == "pcd"){ + pcl::PointCloud pcl_cloud; + pcl::fromROSMsg(*indxpcd[countbag], pcl_cloud); + + // 保存点云数据到PCD文件 + pcl::io::savePCDFileASCII(saveFilePathStr, pcl_cloud); + }else if (NameType == "ply"){ + pcl::PointCloud pcl_cloud; + pcl::fromROSMsg(*indxpcd[countbag], pcl_cloud); + + // 保存点云数据到PCD文件 + pcl::io::savePLYFileASCII(saveFilePathStr, pcl_cloud); + }else if(NameType == "bin"){ + pcl::PointCloud pcl_cloud; + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + + pcl::fromROSMsg(*indxpcd[countbag], *cloud); + std::ofstream bin_file(saveFilePathStr.c_str(),std::ios::out|std::ios::binary|std::ios::app); + if(!bin_file.good()) std::cout<<"Couldn't open "<points.size (); ++i) + { + bin_file.write(reinterpret_cast(&cloud->points[i].x), sizeof(float)); + bin_file.write(reinterpret_cast(&cloud->points[i].y), sizeof(float)); + bin_file.write(reinterpret_cast(&cloud->points[i].z), sizeof(float)); + } + + bin_file.close(); + }else{ + QMessageBox msgBox; + msgBox.setWindowTitle("Tips"); + msgBox.setText("file type error"); + msgBox.resize(2000, 1000); + msgBox.exec(); + continue; + } + // std::ifstream file(saveFilePathStr.c_str()); + + + 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; + } + // QMessageBox::information(this, "tips", "Move Slider to Choose End!"); + if(is_saveMulFinish){ + frameEnd = countbag; + // std::string temFileStr = pcdFilePathStr; + // temFileStr.resize(pcdFilePathStr.length() - 4); + if(frameStart > frameEnd){ + int tem = frameEnd; + frameEnd = frameStart; + frameStart = tem; + } + std::ifstream file(SaveMulfileName.toStdString().c_str()); + if (!file.good()) { + newBag.open(SaveMulfileName.toStdString(), rosbag::bagmode::Write); + + for(int i = frameStart; i < frameEnd; i++) + newBag.write("/scan", ros::Time::now(), indxpcd[i]); + + is_saveMulFinish = false; + is_saveMul = false; + saveFlag = false; + newBag.close(); + }else{ + + } + } + else{ + if(!saveFlag){ + std::string filetype = find(SaveMulfileName.toStdString()); + if(SaveMulfileName.isEmpty()){ + QMessageBox msgBox; + msgBox.setWindowTitle("Tips"); + msgBox.setText("Save failure, No file is selected, Please seve again!"); + msgBox.resize(2000, 1000); + msgBox.exec(); + is_saveMul = false; + continue; + }else if(filetype != "bag"){ + QMessageBox msgBox; + msgBox.setWindowTitle("Tips"); + msgBox.setText("file type error"); + msgBox.resize(2000, 1000); + msgBox.exec(); + is_saveMul = false; + continue; + } + frameStart = countbag; + saveFlag = true; + }else{ + ; + } + } + if(ismoved){ + double valuedouble = progress->value(); + countbag = round((valuedouble/100)*indxpcdsize); + scan_pubpcd.publish(*indxpcd[countbag]); + progress->setSliderPosition((countbag/indxpcdsize*100)); + ismoved = false; + } + else{ + scan_pubpcd.publish(*indxpcd[countbag]); + progress->setSliderPosition((countbag/indxpcdsize*100)); + r.sleep(); + ischanged = false; + } + //Reached the end of the file, but did not stop the multiple frame saving operation + if(countbag == indxpcdsize){ + // newBag.close(); + frameEnd = indxpcdsize; + is_saveMulFinish = true; + + // countbag = 0; + + // ispauseclick = false; + } + + continue; + } + ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + if(ismoved)// 手拖动时改变进度条 + { + end = false; + int value = (progress->value()/100)*indxpcdsize; + double valuedouble = progress->value(); + countbag = round((valuedouble/100)*indxpcdsize); + if(countbag >= indxpcdsize)// 直接重新开始for循环 + { + countbag = indxpcdsize-1; + progress->setSliderPosition(100); + ispauseclick = false; + } + ismoved = false; + } + else if(ischanged)// 手戳动时改变进度条 + { + end = false; + int value = (progress->value()/100)*indxpcdsize; + double valuedouble = progress->value(); + countbag = round((valuedouble/100)*indxpcdsize); + if(countbag >= indxpcdsize)// 直接重新开始for循环 + { + countbag = indxpcdsize-1; + progress->setSliderPosition(100); + ispauseclick = false; + } + scan_pubpcd.publish(*indxpcd[countbag]); + countbag++; + ischanged = false; + r.sleep(); + continue; + // ismoved = false; + } + if(!ispauseclick || end) + { + if(is_next){ + if(countbag == indxpcdsize) + countbag = 0; + else + countbag++; + scan_pubpcd.publish(*indxpcd[countbag]); + progress->setSliderPosition(ceil(countbag/indxpcdsize*100)); + r.sleep(); + ischanged = false; + is_next = false; + // continue; + } + if(is_previous){ + if(countbag > 0) + countbag--; + scan_pubpcd.publish(*indxpcd[countbag]); + progress->setSliderPosition(ceil(countbag/indxpcdsize*100)); + r.sleep(); + ischanged = false; + is_previous = false; + // continue; + } + if(countbagsliderPosition()<=100 && countbagvalue/indxpcdsize*100<=100) + { + progress->setSliderPosition(ceil(countbagvalue/indxpcdsize*100)); + } + countbag++; + ischanged = false; + r.sleep(); + } + } + } +} + +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(){// ********************************************************************************************************************* + QIcon play_png(button_icon_filename.arg("play")); + //QIcon play_png("/home/laijunwen/show/src/point-cloud-visualization/librviz_tutorial/sources/images/play.png"); + play->setIcon(play_png); + threadout = true; + if(threadout)// 清空播放容器 + { + stored = true; + indx.clear(); + } + progress->setSliderPosition(0); + isthreadplaying = false; + playagain = false; + QString tempath; + tempath = QFileDialog::getOpenFileName(nullptr, "select file", "/home", "bag(*.bag);;所有文件 (*)"); + if(tempath.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{ + filePath = tempath; + 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; + is_rateChanged = true; + } + else{ + qDebug() << "speed change failed"; + } +} + + +void MyViz::pcdFile()// ********************************************************************************************************************* +{ + char** argv;// 字符串内容(输入) + int argc=0;// 字符串个数(输入) + ros::init (argc, argv, "UandBdetect"); + ros::NodeHandle nh; + ros::Publisher pcl_pub = nh.advertise ("pcd_output", 1); + pcl::PointCloud cloud; + sensor_msgs::PointCloud2 output; + std::string pcdFilePathStr = filePath.toStdString(); + pcl::io::loadPCDFile (pcdFilePathStr, cloud); //修改自己pcd文件所在路径 + //Convert the cloud to ROS message + pcl::toROSMsg(cloud, output); + output.header.frame_id = "world";//this has been done in order to be able to visualize our PointCloud2 message on the RViz visualizer + + ros::Rate loop_rate(1); + + pcl_pub.publish(output); + ros::spinOnce(); + loop_rate.sleep(); +} + + +std::string MyViz::find(const std::string Path) +{ + size_t lastDotPos = Path.find_last_of('.'); + std::string fileExtension; + if (lastDotPos != std::string::npos) + // 提取后缀 + fileExtension = Path.substr(lastDotPos + 1); + return fileExtension; +} + + +//********************************************************************************************************************* +void MyViz::plyFile() +{ + char** argv;// 字符串内容(输入) + int argc=0;// 字符串个数(输入) + ros::init (argc, argv, "UandBdetect"); + ros::NodeHandle nh; + ros::Publisher pcl_pub = nh.advertise ("ply_output", 1); + pcl::PointCloud cloud; + sensor_msgs::PointCloud2 output; + std::string pcdFilePathStr = filePath.toStdString(); + pcl::io::loadPLYFile (pcdFilePathStr, cloud); //修改自己pcd文件所在路径 + //Convert the cloud to ROS message + pcl::toROSMsg(cloud, output); + output.header.frame_id = "world";//this has been done in order to be able to visualize our PointCloud2 message on the RViz visualizer + //！！！这一步需要注意，是后面rviz的 fixed_frame !!!敲黑板，画重点。 + ros::Rate loop_rate(1); + // while (ros::ok()) + // { + pcl_pub.publish(output); + ros::spinOnce(); + loop_rate.sleep(); + // } +} + +void MyViz::bin() +{ + std::string binFilePathStr = filePath.toStdString(); + std::ifstream inFile(binFilePathStr, std::ios::binary); + if (!inFile.is_open()) + { + std::cerr << "Failed to open input binary file." << std::endl; + + } + pcl::PointCloud::Ptr cloud1(new pcl::PointCloud); + while (!inFile.eof()) + { + pcl::PointXYZ point; + inFile.read(reinterpret_cast(&point.x), sizeof(float)); + inFile.read(reinterpret_cast(&point.y), sizeof(float)); + inFile.read(reinterpret_cast(&point.z), sizeof(float)); + + if (inFile.eof()) + break; + + cloud1->push_back(point); + } + + inFile.close(); + binFilePathStr = binFilePathStr.substr(0,binFilePathStr.size()-4); + char** argv;// 字符串内容(输入) + int argc=0;// 字符串个数(输入) + ros::init (argc, argv, "UandBdetect"); + ros::NodeHandle nh; + ros::Publisher pcl_pub = nh.advertise ("bin_output", 1); + pcl::PointCloud cloud; + sensor_msgs::PointCloud2 output; + binFilePathStr = binFilePathStr+".pcd"; + pcl::toROSMsg(*cloud1, output); + output.header.frame_id = "world";//this has been done in order to be able to visualize our PointCloud2 message on the RViz visualizer + ros::Rate loop_rate(1); + pcl_pub.publish(output); + ros::spinOnce(); + loop_rate.sleep(); + +} + +void MyViz::pcd2bin (std::string &in_file, std::string& out_file){ + //Create a PointCloud value + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + + //Open the PCD file + if (pcl::io::loadPCDFile (in_file, *cloud) == -1) + { + PCL_ERROR ("Couldn't read in_file\n"); + } + //Create & write .bin file + std::ofstream bin_file(out_file.c_str(),std::ios::out|std::ios::binary|std::ios::app); + if(!bin_file.good()) std::cout<<"Couldn't open "<points.size (); ++i) + { + bin_file.write(reinterpret_cast(&cloud->points[i].x), sizeof(float)); + bin_file.write(reinterpret_cast(&cloud->points[i].y), sizeof(float)); + bin_file.write(reinterpret_cast(&cloud->points[i].z), sizeof(float)); + } + + bin_file.close(); +} + +void MyViz::pcd2ply(std::string &in_file, std::string& out_file){ + pcl::PointCloud cloud; + + pcl::io::loadPCDFile (in_file, cloud); //修改自己pcd文件所在路径 + // pcl::PLYWriter writer; + // writer.write(out_file, cloud); + + pcl::io::savePLYFileASCII(out_file, cloud); +} + +void MyViz::ply2pcd(std::string &in_file, std::string& out_file){ + pcl::PointCloud cloud; + + pcl::io::loadPLYFile (in_file, cloud); //修改自己ply文件所在路径 + + pcl::io::savePCDFileASCII(out_file, cloud); +} + +void MyViz::ply2bin(std::string &in_file, std::string& out_file){ + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + if (pcl::io::loadPLYFile (in_file, *cloud) == -1) + { + PCL_ERROR ("Couldn't read in_file\n"); + } + //Create & write .bin file + std::ofstream bin_file(out_file.c_str(),std::ios::out|std::ios::binary|std::ios::app); + if(!bin_file.good()) std::cout<<"Couldn't open "<points.size (); ++i) + { + bin_file.write(reinterpret_cast(&cloud->points[i].x), sizeof(float)); + bin_file.write(reinterpret_cast(&cloud->points[i].y), sizeof(float)); + bin_file.write(reinterpret_cast(&cloud->points[i].z), sizeof(float)); + } + + bin_file.close(); +} + +void MyViz::bin2pcd(std::string &in_file, std::string& out_file){ + std::fstream input(in_file.c_str(), std::ios::in | std::ios::binary); + if(!input.good()){ + std::cerr << "Couldn't read in_file: " << in_file << endl; + } + + pcl::PointCloud::Ptr points (new pcl::PointCloud); + + int i; + for (i=0; input.good() && !input.eof(); i++) { + pcl::PointXYZI point; + input.read((char *) &point.x, 3*sizeof(float)); + input.read((char *) &point.intensity, sizeof(float)); + points->push_back(point); + } + input.close(); + + pcl::io::savePCDFileASCII(out_file, *points); +} + +void MyViz::bin2ply(std::string &in_file, std::string& out_file){ + std::fstream input(in_file.c_str(), std::ios::in | std::ios::binary); + if(!input.good()){ + std::cerr << "Couldn't read in_file: " << in_file << endl; + } + + pcl::PointCloud::Ptr points (new pcl::PointCloud); + + int i; + for (i=0; input.good() && !input.eof(); i++) { + pcl::PointXYZI point; + input.read((char *) &point.x, 3*sizeof(float)); + input.read((char *) &point.intensity, sizeof(float)); + points->push_back(point); + } + input.close(); + + pcl::io::savePLYFileASCII(out_file, *points); + // pcl::PLYWriter writer; + // writer.write(out_file, *points); +} + +void MyViz::TransformeType(){ + if(filePath.isEmpty()){ + QMessageBox msgBox; + msgBox.setWindowTitle("Tips"); + msgBox.setText("Not select File"); + msgBox.resize(2000, 1000); + msgBox.exec(); + return; + } + std::string na = find(filePath.toStdString()); + if(na == "bag") + { + QMessageBox msgBox; + msgBox.setWindowTitle("Tips"); + msgBox.setText("bag file can't transform from here!!"); + msgBox.resize(2000, 1000); + msgBox.exec(); + return; + } + QString SavefileName = QFileDialog::getSaveFileName(nullptr, "Save File", "/home","files(*)"); + qDebug() << SavefileName; + if(SavefileName.isEmpty()){ + QMessageBox msgBox; + msgBox.setWindowTitle("Tips"); + msgBox.setText("Not select File"); + msgBox.resize(2000, 1000); + msgBox.exec(); + return; + } + + std::string SaveFileNameStr = SavefileName.toStdString(); + std::string aftertypeName = find(SaveFileNameStr); + std::string beforetypeName = find(filePath.toStdString()); + + if(beforetypeName == "pcd"){ + if(aftertypeName == "pcd"){ + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + //Open the PCD file + if (pcl::io::loadPCDFile (filePath.toStdString(), *cloud) == -1) + PCL_ERROR ("Couldn't read in_file\n"); + if(pcl::io::savePCDFileASCII(SaveFileNameStr, *cloud) != -1) + std::cout<<" --- save " << SaveFileNameStr << std::endl; + }else if(aftertypeName == "ply"){ + std::string temstr = filePath.toStdString(); + pcd2ply(temstr, SaveFileNameStr); + }else if(aftertypeName == "bin"){ + std::string temstr = filePath.toStdString(); + pcd2bin(temstr, SaveFileNameStr); + } + + }else if(beforetypeName == "ply"){ + if(aftertypeName == "pcd"){ + std::string temstr = filePath.toStdString(); + ply2pcd(temstr, SaveFileNameStr); + } + else if(aftertypeName == "ply"){ + pcl::PointCloud cloud; + + std::string plyFilePathStr = filePath.toStdString(); + pcl::io::loadPLYFile (plyFilePathStr, cloud); //修改自己ply文件所在路径 + // pcl::PLYWriter writer; + // writer.write(SaveFileNameStr, cloud); + if(pcl::io::savePLYFileASCII(SaveFileNameStr, cloud) != -1) + std::cout<<" --- save " << SaveFileNameStr << std::endl; + }else if(aftertypeName == "bin"){ + std::string temstr = filePath.toStdString(); + ply2bin(temstr, SaveFileNameStr); + } + }else if(beforetypeName == "bin"){ + if(aftertypeName == "pcd"){ + std::string temstr = filePath.toStdString(); + bin2pcd(temstr, SaveFileNameStr); + } + else if(aftertypeName == "ply"){ + std::string temstr = filePath.toStdString(); + bin2ply(temstr, SaveFileNameStr); + } + // else if(aftertypeName == "bin"){ + // std::string binFilePathStr = filePath.toStdString(); + // std::ifstream inFile(binFilePathStr, std::ios::binary); + // if (!inFile.is_open()) + // std::cerr << "Failed to open input binary file." << std::endl; + // pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + // while (!inFile.eof()){ + // pcl::PointXYZ point; + // inFile.read(reinterpret_cast(&point.x), sizeof(float)); + // inFile.read(reinterpret_cast(&point.y), sizeof(float)); + // inFile.read(reinterpret_cast(&point.z), sizeof(float)); + + // if (inFile.eof()) + // break; + // cloud->push_back(point); + // } + // inFile.close(); + // std:: ofstream bin_file(filePath.toStdString().c_str(),std::ios::out|std::ios::binary|std::ios::app); + // if(!bin_file.good()) std::cout<<"Couldn't open "<points.size (); ++i){ + // bin_file.write((char*)&cloud->points[i].x,3*sizeof(float)); + // bin_file.write((char*)&cloud->points[i].intensity,sizeof(float)); + // } + // bin_file.close(); + // } + }else{ + QMessageBox msgBox; + msgBox.setWindowTitle("Tips"); + msgBox.setText("File type error"); + msgBox.resize(2000, 1000); + msgBox.exec(); + } +} + diff --git a/librviz_tutorial/src/myviz.h b/point_visual/src/myviz.h similarity index 66% rename from librviz_tutorial/src/myviz.h rename to point_visual/src/myviz.h index 9de4261..9160e0b 100644 --- a/librviz_tutorial/src/myviz.h +++ b/point_visual/src/myviz.h @@ -32,20 +32,22 @@ #include -#include +#include #include -#include -#include"std_msgs/String.h" -#include +#include +#include "std_msgs/String.h" +#include #include -#include +#include #include #include #include -#include +#include #include #include +#include + #include #include"loginDialog.h" @@ -56,8 +58,8 @@ class RenderPanel; class VisualizationManager; class ToolManager; class PropertyTreeWidget; -class ToolManager; -class PropertyTreeWidget; +class ViewManager; +class ViewController; } // BEGIN_TUTORIAL @@ -70,64 +72,93 @@ public: virtual ~MyViz(); void subCallback(const std_msgs::String& msg); void pubThread(); - void PlayCircle(double index); // 圆圈线程执行函数 + void PlayCircles(); // 圆圈线程执行函数 rosbag::View *view; + rosbag::View *view_; + private Q_SLOTS: // QT信号和槽 void setThickness( int thickness_percent ); void setCellSize( int cell_size_percent ); void setCloudTopic(const QString &newTopic); - void setLaserTopic(const QString &newTopic); - void setCloudSize(int cloudsize); - void setLaserSize(int lasersize); + void setCloudSize(const QString& size); + void setCloudAlpha(const QString& text); + void setLaserAlpha(const QString& laserAlpha); + void setLaserTopic(const QString &newTopic); + void setLaserSize(const QString & laserSize); void LaserColorChanged(int index); // 雷达颜色 void GridColorChanged(int index); // 格栅颜色 + void CloudColorChanged(int index); void setFixedFrame(const QString &FixedFrame); void setBackgroundColor(const QString& bg_color); // 背景颜色 void AxesDisplayChanged(int index); // axe - void CircleDisplayChanged(const QString& text); // 圆圈变化 void Tree_Display(QGridLayout* layout, int index); void login_button_clicked(); void quit_login_button_clicked(); void slot_select(); // 切换至select模式 void slot_move_camera(); + void CircleDisplayAdded(); // 增加圆圈按钮 + void CircleDisplayDeleted(); // 删除圆圈按钮 + void ViewZero(); // 视角回正 + void ViewLeft(); // 视角左转 + void ViewRight(); // 视角右转 + void ViewTop(); // 视角俯视 // off double extractDuration(const QByteArray& output); void selectFile(); void SpeedChanged(int index); void openFile(); + void pcdFile(); + void plyFile(); + void bin(); + std::string find(const std::string Path); + + void pcd2ply (std::string &in_file, std::string& out_file); + void pcd2bin (std::string &in_file, std::string& out_file); + void ply2pcd (std::string &in_file, std::string& out_file); + void ply2bin (std::string &in_file, std::string& out_file); + void bin2pcd (std::string &in_file, std::string& out_file); + void bin2ply (std::string &in_file, std::string& out_file); + void TransformeType(); + private: rviz::VisualizationManager* manager_; // rviz::RenderPanel* render_panel_; rviz::ToolManager* tool_manager_; + rviz::ViewManager* view_man_; // 视角总类 + rviz::ViewController* viewController; // 视角控制类 rviz::Display* grid_; rviz::Display* cloud; rviz::Display* laser_; rviz::Display* axes_display; rviz::Display* marker_; - rviz::PropertyTreeWidget* tree_widget_ ; + rviz::PropertyTreeWidget* select_tree_widget_ ; // select框 + + // rviz::PropertyTreeWidget* view_tree_widget_ ; ros::NodeHandle nh; ros::Subscriber sub; ros::Publisher pub; std::thread pub_thread; std::vector color_name; // 颜色的名称集合 + // std::vector button_name; // 带图标按钮的名称集合 QString color_icon_filename; + QString button_icon_filename; LoginDialog* dlg; bool landed; QString cur_fixed_frame; + int circle_index; + std::map circle_radius; // off - bool isclicked;// 用于判断play第几次按下play键 bool ispauseclick;// 用于判断暂停键第几次按下 int count;// 用于记录timeout的次数 bool sliderreleased;// 用于判断是否拖动了进度条 bool playagain;// 用于检测是否多次按下play按钮 bool isthreadplaying;// 用于标识线程是否在运行 - bool abs;// 用于单独开线程？ - bool stored;// 用于存储一次容器,只在第一次play的时候存储容器 + bool stored = false;// 用于存储一次容器,只在第一次play的时候存储容器 bool is_pause = false; bool is_reset = false; bool is_end = false; @@ -142,17 +173,30 @@ private: bool threadout = false; bool ischanged = false; bool ismoved = false; - double speed_rate; + + bool sliderpause = false; + bool is_next = false; + bool is_previous = false; + bool flag = false; + bool pointcloud2 = false; + double speed_rate = 10; + QPushButton* play; QSlider* progress; QString filePath; - double duration; - int remain; + QString Topic; + QString SavefileName; + QString SaveMulfileName; + + double sliderPosition = 0; int prog_num = 0; std::vector speed_name; ros::Publisher scan_pub; - std::vector indx;// 存储节点信息 - int countbag;// 存储播放的具体位置 - double indxsize; + ros::Publisher scan_pubpcd; + std::vector indx;// 存储laser节点信息 + std::vector indxpcd;// 存储cloud节点信息 + int countbag = 0;// 存储播放的具体位置 + double indxsize = 0; + double indxpcdsize = 0; int test = 0; diff --git a/librviz_tutorial/src/node.cpp b/point_visual/src/node.cpp similarity index 100% rename from librviz_tutorial/src/node.cpp rename to point_visual/src/node.cpp diff --git a/librviz_tutorial/tools/color.cpp b/point_visual/tools/color.cpp similarity index 100% rename from librviz_tutorial/tools/color.cpp rename to point_visual/tools/color.cpp diff --git a/librviz_tutorial/tools/color.h b/point_visual/tools/color.h similarity index 100% rename from librviz_tutorial/tools/color.h rename to point_visual/tools/color.h