diff --git a/src/doc/conf.py b/src/doc/conf.py new file mode 100644 index 0000000..bb18b12 --- /dev/null +++ b/src/doc/conf.py @@ -0,0 +1,25 @@ +import sys, os + +sys.path += [ os.path.abspath( '.' )] + +extensions = [ 'sphinx.ext.extlinks', + 'tutorialformatter' ] + +# The master toctree document. +master_doc = 'index' + +# The suffix of source filenames. +source_suffix = '.rst' + +project = u'librviz_tutorial' + +copyright = u'2012, Willow Garage, Inc' + +# If true, sectionauthor and moduleauthor directives will be shown in the +# output. They are ignored by default. +show_authors = True + +# The name of the Pygments (syntax highlighting) style to use. +pygments_style = 'sphinx' + +extlinks = {'codedir': ('https://github.com/ros-visualization/visualization_tutorials/blob/hydro-devel/librviz_tutorial/%s', '')} diff --git a/src/doc/index.rst b/src/doc/index.rst new file mode 100644 index 0000000..28910ab --- /dev/null +++ b/src/doc/index.rst @@ -0,0 +1,67 @@ +Librviz Tutorial +================ + +Overview +-------- + +RViz is not just a visualizer application, it is also a library! Much +of RViz's functionality can be accessed within your own application by +linking against librviz.so (or whatever your OS likes to call it). + +This tutorial shows a very simple example of creating a 3D visualizer +widget (rviz::RenderPanel), programmatically creating a new Grid +display within it, then using Qt slider controls to adjust a couple of +the grid's properties. The app is called "myviz". + +The source code for this tutorial is in the librviz_tutorial +package. You can check out the source directly or (if you use Ubuntu) +you can just apt-get install the pre-compiled Debian package like so:: + + sudo apt-get install ros-hydro-visualization-tutorials + +The running application looks like this: + +.. image:: myviz.png + +The Code +-------- + +The code for myviz is in these files: +:codedir:`src/main.cpp`, +:codedir:`src/myviz.h`, and +:codedir:`src/myviz.cpp`. + +main.cpp +^^^^^^^^ + +The full text of main.cpp is here: :codedir:`src/main.cpp` + +.. tutorial-formatter:: ../main.cpp + +myviz.h +^^^^^^^ + +The full text of myviz.h is here: :codedir:`src/myviz.h` + +.. tutorial-formatter:: ../myviz.h + +myviz.cpp +^^^^^^^^^ + +The full text of myviz.cpp is here: :codedir:`src/myviz.cpp` + +.. tutorial-formatter:: ../myviz.cpp + +Building +-------- + +The full text of CMakeLists.txt is here: :codedir:`CMakeLists.txt` + +.. tutorial-formatter:: ../../CMakeLists.txt + +Running +------- + +Just type:: + + rosrun librviz_tutorial myviz diff --git a/src/doc/myviz.png b/src/doc/myviz.png new file mode 100644 index 0000000..ab623d2 Binary files /dev/null and b/src/doc/myviz.png differ diff --git a/src/doc/tutorialformatter.py b/src/doc/tutorialformatter.py new file mode 100644 index 0000000..8260942 --- /dev/null +++ b/src/doc/tutorialformatter.py @@ -0,0 +1,132 @@ +""" + tutorialformatter + =========================== + + This extension provides a directive to include a source code file + in a document, but with certain comments from the file formatted + as regular document text. This allows code for a tutorial to look like: + + /// BEGIN_TUTORIAL + /// This next line adds one. + i = i + 1; + /// Then we need to double it. + i = i * 2; + /// END_TUTORIAL + + And have it formatted as + + This next line adds one.:: + i = i + 1; + + Then we need to double it.:: + i = i * 2; + + The special-looking comment character sequence at the start of + each text line can be anything not starting or ending with + whitespace. tutorialformatter starts by scanning the file for the + string BEGIN_TUTORIAL. When it finds it, it takes all the + characters before BEGIN_TUTORIAL on that line, strips whitespace + from the left, and uses that as the text marker. So this would + also be fine: + + #My Tutorial# BEGIN_TUTORIAL + #My Tutorial# This next line adds one. + i = i + 1 + #My Tutorial# Then we need to double it. + i = i * 2 + #My Tutorial# END_TUTORIAL + + .. moduleauthor:: Dave Hershberger +""" + +__version__ = '0.1.0' + +import os +from docutils.parsers import rst +from docutils.parsers.rst.directives import flag, unchanged +from docutils.statemachine import string2lines +from pygments.lexers import get_lexer_for_filename + +class TutorialFormatterDirective(rst.Directive): + has_content = False + final_argument_whitespace = True + required_arguments = 1 + + option_spec = dict(shell=flag, prompt=flag, nostderr=flag, + in_srcdir=flag, extraargs=unchanged, + until=unchanged) + + def run(self): + filename = self.arguments[0] + text_tag = None + tag_len = 0 + + filepath = self.state.document.settings.env.srcdir + absfilename = os.path.join( filepath, filename ) + if absfilename.endswith('.h'): + language = 'c++' + elif absfilename.endswith('CMakeLists.txt'): + language = 'cmake' + else: + try: + language = get_lexer_for_filename( absfilename ).name.lower() + if language == 'text only': + language = 'none' + except: + language = 'none' + code_prefix = '\n.. code-block:: ' + language + '\n\n' + code_suffix = '\n' + + print "tutorial-formatter running on", absfilename + file_ = open( absfilename, 'r' ) + text_to_process = "" + current_block = "" + in_code = False + in_text = False + in_tutorial = False + for line in file_: + if not in_tutorial: + begin_pos = line.find( 'BEGIN_TUTORIAL' ) + if begin_pos != -1: + text_tag = line[:begin_pos].lstrip() + tag_len = len( text_tag ) + in_tutorial = True + continue + if line.find( 'END_TUTORIAL' ) != -1: + break + stripped = line.lstrip() + if stripped.startswith( text_tag.strip() ): + if in_code: + text_to_process += code_prefix + current_block + code_suffix + current_block = "" + in_code = False + in_text = True + addition = stripped[tag_len:] + if addition == '' or addition[-1] != '\n': + addition += '\n' + current_block += addition + else: + if in_text: + text_to_process += current_block + current_block = "" + in_text = False + in_code = True # Code to show begins right after tagged text + if in_code: + current_block += ' ' + line + if in_code: + text_to_process += code_prefix + current_block + code_suffix + elif in_text: + text_to_process += current_block + + # Debug writes... + #print 'text_to_process =' + #print text_to_process + #print '= text_to_process' + + lines = string2lines( text_to_process ) + self.state_machine.insert_input( lines, absfilename ) + + return [] + +def setup(app): + app.add_directive('tutorial-formatter', TutorialFormatterDirective) diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..c6ad557 --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,56 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +// BEGIN_TUTORIAL + +// The main() for this "myviz" example is very simple, it just +// initializes ROS, creates a QApplication, creates the top-level +// widget (of type "MyViz"), shows it, and runs the Qt event loop. + +#include +#include +#include "myviz.h" + +int main(int argc, char **argv) +{ + if( !ros::isInitialized() ) + { + ros::init( argc, argv, "myviz", ros::init_options::AnonymousName ); + } + + QApplication app( argc, argv ); + + MyViz* myviz = new MyViz(); + myviz->setGeometry(0, 0, 2000, 1000); + myviz->show(); + + app.exec(); + + delete myviz; +} diff --git a/src/myviz.cpp b/src/myviz.cpp new file mode 100644 index 0000000..cee9b81 --- /dev/null +++ b/src/myviz.cpp @@ -0,0 +1,270 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include "rviz/visualization_manager.h" +#include "rviz/render_panel.h" +#include "rviz/display.h" + +#include "myviz.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); + //pub_thread.join(); + // Construct and lay out labels and slider controls. + QLabel* thickness_label = new QLabel( "Line Thickness" ); + QSlider* thickness_slider = new QSlider( Qt::Horizontal ); + thickness_slider->setMinimum( 1 ); + thickness_slider->setMaximum( 100 ); + QLabel* cell_size_label = new QLabel( "Cell Size" ); + QSlider* cell_size_slider = new QSlider( Qt::Horizontal ); + cell_size_slider->setMinimum( 1 ); + cell_size_slider->setMaximum( 100 ); + QLabel* Topic=new QLabel("TOPIC:"); + QLineEdit* Topic_text=new QLineEdit(); + QGridLayout* controls_layout = new QGridLayout(); + QLabel* Size=new QLabel("Cloud Size"); + QSpinBox* size=new QSpinBox(); + + /*button goto file begining*/ + QPushButton* but_reset = new QPushButton("reset", this); + /*button goto file ending*/ + QPushButton* but_ending = new QPushButton("end", this); + + //TODO: set button color + //button->setStyleSheet("background-color: red; color: white;"); + //TODO: set shortcut key + //button->setShortcut(QKeySequence(Qt::CTRL + Qt::Key_A)); + + + //enable button + but_reset->setEnabled(true); + but_ending->setEnabled(true); + + but_reset->setFixedSize(100, 30); + but_ending->setFixedSize(100, 30); + + controls_layout->addWidget( thickness_label, 0, 0 ); + controls_layout->addWidget( thickness_slider, 0, 1 ); + controls_layout->addWidget( cell_size_label, 0, 2 ); + controls_layout->addWidget( cell_size_slider, 0, 3 ); + + //button locatin + controls_layout->addWidget( but_reset, 1, 0 ); + controls_layout->addWidget( but_ending, 1, 1 ); + + controls_layout->addWidget(Topic,0,4); + controls_layout->addWidget(Topic_text,0,5); + controls_layout->addWidget(Size,0,6); + controls_layout->addWidget(size,0,7); + + + // Construct and lay out render panel. + render_panel_ = new rviz::RenderPanel(); + QVBoxLayout* main_layout = new QVBoxLayout; + main_layout->addLayout( controls_layout ); + main_layout->addWidget( render_panel_ ); + + // Set the top-level layout for this MyViz widget. + setLayout( main_layout ); + + // Make signal/slot connections. + connect( thickness_slider, SIGNAL( valueChanged( int )), this, SLOT( setThickness( int ))); + connect( cell_size_slider, SIGNAL( valueChanged( int )), this, SLOT( setCellSize( int ))); + //connect(Topic_text,SIGNAL(textChanged(const QString &)),this,SLOT(setCloudTopic(const QString &))); + + //topic change signal + connect(Topic_text,SIGNAL(textChanged(const QString &)),this,SLOT(setlaserTopic(const QString &))); + //button signal + connect(but_ending, &QPushButton::clicked, this, [&]() { + QStringList arguments, infoarg; + QString time; + QString file_path = "/home/noerror/123/catkin_for_ros/src/librviz_tutorial/data/result.bag"; + QProcess *rosbaginfoProcess = new QProcess(this); + + infoarg << "info" << file_path; + rosbaginfoProcess->start("rosbag", infoarg); + + if(rosbaginfoProcess->waitForFinished()){ + QByteArray output = rosbaginfoProcess->readAllStandardOutput(); + 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(); //qstring convert to double + time = QString::number(number-0.1); + qDebug() << time; + } + + arguments << "play" << file_path << "--start" << time; + QProcess *rosbagProcess = new QProcess(this); + //QProcess::execute("rosbag play /home/noerror/123/catkin_for_ros/src/librviz_tutorial/data/result.bag"); + + rosbagProcess->start("rosbag", arguments); + + }); + connect(but_reset, &QPushButton::clicked, this, [&]() { + QStringList arguments, infoarg; + QString time; + QString file_path = "/home/noerror/123/catkin_for_ros/src/librviz_tutorial/data/result.bag"; + + arguments << "play" << file_path; + QProcess *rosbagProcess = new QProcess(this); + //QProcess::execute("rosbag play /home/noerror/123/catkin_for_ros/src/librviz_tutorial/data/result.bag"); + + rosbagProcess->start("rosbag", arguments); + + }); + connect(size,SIGNAL(valueChanged(int)),this,SLOT(setCloudSize(int))); + // 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_ ); + render_panel_->initialize( manager_->getSceneManager(), manager_ ); + //manager_->setFixedFrame("world"); + + /*TODO: laser data*/ + manager_->setFixedFrame("laser"); + manager_->initialize(); + manager_->startUpdate(); + + // 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 ); + + /*TODO: laser data display*/ + laser = manager_->createDisplay( "rviz/LaserScan", "laser", true ); + ROS_ASSERT( laser != NULL ); + //set defualt topic to /scan + laser->subProp("Topic")->setValue("/scan"); + laser->subProp("Decay Time")->setValue("0"); + + + 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::yellow ) ); + + // Initialize the slider values. + thickness_slider->setValue( 5 ); + cell_size_slider->setValue( 10 ); +} + +// 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("Size (Pixels)")->setValue(cloudsize); + } +} +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::setlaserTopic(const QString & lasertopic){ + if(laser!=NULL){ + laser->subProp("Topic")->setValue(lasertopic); + manager_->startUpdate(); + } + +} +void MyViz::PushButtonBegining(const QString & things){ + + QProcess::execute("rosbag play /home/noerror/123/catkin_for_ros/src/librviz_tutorial/data/result.bag "); + +} \ No newline at end of file diff --git a/src/myviz.h b/src/myviz.h new file mode 100644 index 0000000..be8d46f --- /dev/null +++ b/src/myviz.h @@ -0,0 +1,79 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef MYVIZ_H +#define MYVIZ_H + +#include +#include +#include +#include"std_msgs/String.h" +#include +namespace rviz +{ +class Display; +class RenderPanel; +class VisualizationManager; +} + +// BEGIN_TUTORIAL +// Class "MyViz" implements the top level widget for this example. +class MyViz: public QWidget +{ +Q_OBJECT +public: + MyViz( QWidget* parent = 0 ); + virtual ~MyViz(); + void subCallback(const std_msgs::String& msg); + void pubThread(); +private Q_SLOTS: + void setThickness( int thickness_percent ); + void setCellSize( int cell_size_percent ); + void setCloudTopic(const QString &newTopic); + void setCloudSize(int cloudsize); + void setlaserTopic(const QString &lasertopic); + void PushButtonBegining(const QString & things); + +private: + rviz::VisualizationManager* manager_; + rviz::RenderPanel* render_panel_; + rviz::Display* grid_; + rviz::Display* cloud; + + /*TODO: laser data*/ + rviz::Display* laser; + /*TODO: create new thread*/ + QProcess *rosbagProcessStart, *rosbagProcessEnd, *rosbagProcessReset; + + ros::NodeHandle nh; + ros::Subscriber sub; + ros::Publisher pub; + std::thread pub_thread; +}; +// END_TUTORIAL +#endif // MYVIZ_H diff --git a/src/qthread.h b/src/qthread.h new file mode 100644 index 0000000..2575075 --- /dev/null +++ b/src/qthread.h @@ -0,0 +1,20 @@ +#include +#include + +class RosbagThread : public QThread { + Q_OBJECT + +public: + void run() override { + // 在这里执行您的rosbag任务 + // 例如,记录数据到ROS bag文件或播放ROS bag文件 + // 使用与前面示例中相似的方式启动rosbag进程 + + QStringList arguments; + QProcess rosbagProcess; + + arguments << "play" << "/home/noerror/123/catkin_for_ros/src/librviz_tutorial/data/result.bag"; + //QProcess::execute("rosbag play /home/noerror/123/catkin_for_ros/src/librviz_tutorial/data/result.bag"); + rosbagProcess.start("rosbag", arguments); + } +}; \ No newline at end of file