├── .gitignore ├── interactive_marker_tutorials ├── CHANGELOG.rst ├── CMakeLists.txt ├── package.xml ├── scripts │ ├── basic_controls.py │ ├── cube.py │ ├── menu.py │ └── simple_marker.py └── src │ ├── basic_controls.cpp │ ├── cube.cpp │ ├── menu.cpp │ ├── point_cloud.cpp │ ├── pong.cpp │ ├── selection.cpp │ └── simple_marker.cpp ├── librviz_tutorial ├── CHANGELOG.rst ├── CMakeLists.txt ├── package.xml ├── rosdoc.yaml └── src │ ├── doc │ ├── conf.py │ ├── index.rst │ ├── myviz.png │ └── tutorialformatter.py │ ├── main.cpp │ ├── myviz.cpp │ └── myviz.h ├── rviz_plugin_tutorials ├── CHANGELOG.rst ├── CMakeLists.txt ├── icons │ └── classes │ │ ├── Imu.png │ │ ├── PlantFlag.png │ │ └── Teleop.png ├── media │ └── flag.dae ├── package.xml ├── plugin_description.xml ├── rosdoc.yaml ├── scripts │ └── send_test_msgs.py └── src │ ├── doc │ ├── building_and_exporting.rst │ ├── conf.py │ ├── display_plugin_tutorial.rst │ ├── flags.png │ ├── imu_arrows.png │ ├── imu_plugin.png │ ├── index.rst │ ├── panel_plugin_tutorial.rst │ ├── real_imu.png │ ├── teleop_in_rviz.png │ ├── teleop_plugin.png │ ├── tool_plugin_tutorial.rst │ └── tutorialformatter.py │ ├── drive_widget.cpp │ ├── drive_widget.h │ ├── flag.h │ ├── imu_display.cpp │ ├── imu_display.h │ ├── imu_visual.cpp │ ├── imu_visual.h │ ├── plant_flag_tool.cpp │ ├── plant_flag_tool.h │ ├── teleop_panel.cpp │ └── teleop_panel.h ├── rviz_python_tutorial ├── CHANGELOG.rst ├── CMakeLists.txt ├── config.myviz ├── doc-src │ ├── conf.py │ ├── index.rst │ ├── myviz.png │ └── tutorialformatter.py ├── myviz.py ├── package.xml └── rosdoc.yaml ├── visualization_marker_tutorials ├── CHANGELOG.rst ├── CMakeLists.txt ├── package.xml └── src │ ├── basic_shapes.cpp │ └── points_and_lines.cpp └── visualization_tutorials ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | *.pyc 3 | bin 4 | build 5 | lib 6 | -------------------------------------------------------------------------------- /interactive_marker_tutorials/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package interactive_marker_tutorials 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.11.2 (2025-04-26) 6 | ------------------- 7 | * Do not forward-declare Ogre types (`#84 `_) 8 | * Contributors: Michael Görner 9 | 10 | 0.11.1 (2025-04-10) 11 | ------------------- 12 | * print_function for py3 (`#61 `_) 13 | * Update maintainers (`#63 `_) 14 | * Contributors: Mabel Zhang, Tobias Fischer 15 | 16 | 0.11.0 (2020-05-13) 17 | ------------------- 18 | 19 | 0.10.4 (2020-05-13) 20 | ------------------- 21 | * Updated to use ``catkin_install_python()`` (`#59 `_) 22 | * Updated required CMake version to avoid CMP0048 warning (`#57 `_) 23 | * Removed unused ``saveMarker()`` (`#47 `_) 24 | * Contributors: Alejandro Hernández Cordero, Bence Magyar, Shane Loretz 25 | 26 | 0.10.3 (2018-05-09) 27 | ------------------- 28 | 29 | 0.10.2 (2018-01-05) 30 | ------------------- 31 | * Normalized quaternions. (`#40 `_) 32 | * Contributors: dhood 33 | 34 | 0.10.1 (2016-04-21) 35 | ------------------- 36 | 37 | 0.10.0 (2016-04-21) 38 | ------------------- 39 | * Added support Qt5 in Kinetic. 40 | * Contributors: William Woodall 41 | 42 | 0.9.2 (2015-09-21) 43 | ------------------ 44 | * Removed deprecated imports. 45 | * Fix Python InteractiveMarkers tutorials to use the correct frame names. 46 | * Updated ``simple_marker.cpp`` by including the time stamp. 47 | Without the time stamp the cube does not show up in rviz and reports an error. 48 | * Contributors: Javier V. Gomez, Robert Haschke, agoudar 49 | 50 | 0.9.1 (2015-01-26) 51 | ------------------ 52 | 53 | 0.9.0 (2014-03-24) 54 | ------------------ 55 | * set myself (william) as maintainer 56 | * Contributors: William Woodall 57 | -------------------------------------------------------------------------------- /interactive_marker_tutorials/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(interactive_marker_tutorials) 3 | 4 | find_package(catkin REQUIRED COMPONENTS interactive_markers roscpp visualization_msgs tf) 5 | 6 | ################################### 7 | ## catkin specific configuration ## 8 | ################################### 9 | ## The catkin_package macro generates cmake config files for your package 10 | ## Declare things to be passed to dependent projects 11 | ## LIBRARIES: libraries you create in this project that dependent projects also need 12 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 13 | ## DEPENDS: system dependencies of this project that dependent projects also need 14 | catkin_package( 15 | CATKIN_DEPENDS interactive_markers roscpp visualization_msgs tf 16 | ) 17 | 18 | ########### 19 | ## Build ## 20 | ########### 21 | 22 | include_directories(include 23 | ${catkin_INCLUDE_DIRS} 24 | ) 25 | 26 | add_executable(simple_marker src/simple_marker.cpp) 27 | target_link_libraries(simple_marker 28 | ${catkin_LIBRARIES} 29 | ) 30 | 31 | add_executable(basic_controls src/basic_controls.cpp) 32 | target_link_libraries(basic_controls 33 | ${catkin_LIBRARIES} 34 | ) 35 | 36 | add_executable(selection src/selection.cpp) 37 | target_link_libraries(selection 38 | ${catkin_LIBRARIES} 39 | ) 40 | 41 | add_executable(pong src/pong.cpp) 42 | target_link_libraries(pong 43 | ${catkin_LIBRARIES} 44 | ) 45 | 46 | add_executable(cube src/cube.cpp) 47 | target_link_libraries(cube 48 | ${catkin_LIBRARIES} 49 | ) 50 | 51 | add_executable(menu src/menu.cpp) 52 | target_link_libraries(menu 53 | ${catkin_LIBRARIES} 54 | ) 55 | 56 | add_executable(point_cloud src/point_cloud.cpp) 57 | target_link_libraries(point_cloud 58 | ${catkin_LIBRARIES} 59 | ) 60 | ############# 61 | ## Install ## 62 | ############# 63 | 64 | catkin_install_python(PROGRAMS 65 | scripts/basic_controls.py 66 | scripts/cube.py 67 | scripts/menu.py 68 | scripts/simple_marker.py 69 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 70 | ) 71 | 72 | ## Mark executables and/or libraries for installation 73 | install(TARGETS 74 | simple_marker 75 | basic_controls 76 | selection 77 | pong 78 | cube 79 | menu 80 | point_cloud 81 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 82 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 83 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 84 | ) 85 | -------------------------------------------------------------------------------- /interactive_marker_tutorials/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | interactive_marker_tutorials 4 | 0.11.2 5 | The interactive_marker_tutorials package 6 | 7 | Mabel Zhang 8 | 9 | BSD 10 | http://ros.org/wiki/interactive_marker_tutorials 11 | 12 | David Gossow 13 | 14 | catkin 15 | 16 | roscpp 17 | interactive_markers 18 | visualization_msgs 19 | tf 20 | 21 | roscpp 22 | interactive_markers 23 | visualization_msgs 24 | tf 25 | 26 | 27 | -------------------------------------------------------------------------------- /interactive_marker_tutorials/scripts/cube.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | Copyright (c) 2011, Willow Garage, Inc. 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of the Willow Garage, Inc. nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | POSSIBILITY OF SUCH DAMAGE. 30 | """ 31 | 32 | import rospy 33 | 34 | from interactive_markers.interactive_marker_server import * 35 | from visualization_msgs.msg import * 36 | from math import sqrt 37 | 38 | positions = list() 39 | 40 | def processFeedback( feedback ): 41 | if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: 42 | #compute difference vector for this cube 43 | x = feedback.pose.position.x 44 | y = feedback.pose.position.y 45 | z = feedback.pose.position.z 46 | index = int(feedback.marker_name) 47 | 48 | if index > len(positions): 49 | return 50 | 51 | dx = x - positions[index][0] 52 | dy = y - positions[index][1] 53 | dz = z - positions[index][2] 54 | 55 | # move all markers in that direction 56 | for i in range(len(positions)): 57 | (mx, my, mz) = positions[i] 58 | d = sqrt(sqrt((x - mx)**2 + (y - my)**2)**2 + (z-mz)**2) 59 | t = 1 / (d*5.0+1.0) - 0.2 60 | if t < 0.0: 61 | t=0.0 62 | positions[i][0] += t*dx 63 | positions[i][1] += t*dy 64 | positions[i][2] += t*dz 65 | 66 | if i == index: 67 | rospy.loginfo( d ) 68 | positions[i][0] = x 69 | positions[i][1] = y 70 | positions[i][2] = z 71 | 72 | pose = geometry_msgs.msg.Pose() 73 | pose.position.x = positions[i][0] 74 | pose.position.y = positions[i][1] 75 | pose.position.z = positions[i][2] 76 | 77 | server.setPose( str(i), pose ) 78 | server.applyChanges() 79 | 80 | def makeBoxControl( msg ): 81 | control = InteractiveMarkerControl() 82 | control.always_visible = True 83 | control.orientation_mode = InteractiveMarkerControl.VIEW_FACING 84 | control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE 85 | control.independent_marker_orientation = True 86 | 87 | marker = Marker() 88 | 89 | marker.type = Marker.CUBE 90 | marker.scale.x = msg.scale 91 | marker.scale.y = msg.scale 92 | marker.scale.z = msg.scale 93 | marker.color.r = 0.65+0.7*msg.pose.position.x 94 | marker.color.g = 0.65+0.7*msg.pose.position.y 95 | marker.color.b = 0.65+0.7*msg.pose.position.z 96 | marker.color.a = 1.0 97 | 98 | control.markers.append( marker ) 99 | msg.controls.append( control ) 100 | return control 101 | 102 | def makeCube(): 103 | side_length = 10 104 | step = 1.0/ side_length 105 | count = 0 106 | 107 | for i in range(side_length): 108 | x = -0.5 + step * i 109 | for j in range(side_length): 110 | y = -0.5 + step * j 111 | for k in range(side_length): 112 | z = step * k 113 | marker = InteractiveMarker() 114 | marker.header.frame_id = "base_link" 115 | marker.scale = step 116 | 117 | marker.pose.position.x = x 118 | marker.pose.position.y = y 119 | marker.pose.position.z = z 120 | 121 | positions.append( [x,y,z] ) 122 | 123 | marker.name = str(count) 124 | makeBoxControl(marker) 125 | 126 | server.insert( marker, processFeedback ) 127 | count += 1 128 | 129 | if __name__=="__main__": 130 | rospy.init_node("cube") 131 | 132 | server = InteractiveMarkerServer("cube") 133 | 134 | rospy.loginfo("initializing..") 135 | makeCube() 136 | server.applyChanges() 137 | 138 | rospy.spin() 139 | 140 | -------------------------------------------------------------------------------- /interactive_marker_tutorials/scripts/menu.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | Copyright (c) 2011, Willow Garage, Inc. 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of the Willow Garage, Inc. nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | POSSIBILITY OF SUCH DAMAGE. 30 | """ 31 | 32 | from __future__ import print_function 33 | 34 | import rospy 35 | 36 | from interactive_markers.interactive_marker_server import * 37 | from interactive_markers.menu_handler import * 38 | from visualization_msgs.msg import * 39 | 40 | server = None 41 | marker_pos = 0 42 | 43 | menu_handler = MenuHandler() 44 | 45 | h_first_entry = 0 46 | h_mode_last = 0 47 | 48 | def enableCb( feedback ): 49 | handle = feedback.menu_entry_id 50 | state = menu_handler.getCheckState( handle ) 51 | 52 | if state == MenuHandler.CHECKED: 53 | menu_handler.setCheckState( handle, MenuHandler.UNCHECKED ) 54 | rospy.loginfo("Hiding first menu entry") 55 | menu_handler.setVisible( h_first_entry, False ) 56 | else: 57 | menu_handler.setCheckState( handle, MenuHandler.CHECKED ) 58 | rospy.loginfo("Showing first menu entry") 59 | menu_handler.setVisible( h_first_entry, True ) 60 | 61 | menu_handler.reApply( server ) 62 | rospy.loginfo("update") 63 | server.applyChanges() 64 | 65 | def modeCb(feedback): 66 | global h_mode_last 67 | menu_handler.setCheckState( h_mode_last, MenuHandler.UNCHECKED ) 68 | h_mode_last = feedback.menu_entry_id 69 | menu_handler.setCheckState( h_mode_last, MenuHandler.CHECKED ) 70 | 71 | rospy.loginfo("Switching to menu entry #" + str(h_mode_last)) 72 | menu_handler.reApply( server ) 73 | print("DONE") 74 | server.applyChanges() 75 | 76 | def makeBox( msg ): 77 | marker = Marker() 78 | 79 | marker.type = Marker.CUBE 80 | marker.scale.x = msg.scale * 0.45 81 | marker.scale.y = msg.scale * 0.45 82 | marker.scale.z = msg.scale * 0.45 83 | marker.color.r = 0.5 84 | marker.color.g = 0.5 85 | marker.color.b = 0.5 86 | marker.color.a = 1.0 87 | 88 | return marker 89 | 90 | def makeBoxControl( msg ): 91 | control = InteractiveMarkerControl() 92 | control.always_visible = True 93 | control.markers.append( makeBox(msg) ) 94 | msg.controls.append( control ) 95 | return control 96 | 97 | def makeEmptyMarker( dummyBox=True ): 98 | global marker_pos 99 | int_marker = InteractiveMarker() 100 | int_marker.header.frame_id = "base_link" 101 | int_marker.pose.position.y = -3.0 * marker_pos 102 | marker_pos += 1 103 | int_marker.scale = 1 104 | return int_marker 105 | 106 | def makeMenuMarker( name ): 107 | int_marker = makeEmptyMarker() 108 | int_marker.name = name 109 | 110 | control = InteractiveMarkerControl() 111 | 112 | control.interaction_mode = InteractiveMarkerControl.BUTTON 113 | control.always_visible = True 114 | 115 | control.markers.append( makeBox( int_marker ) ) 116 | int_marker.controls.append(control) 117 | 118 | server.insert( int_marker ) 119 | 120 | def deepCb( feedback ): 121 | rospy.loginfo("The deep sub-menu has been found.") 122 | 123 | def initMenu(): 124 | global h_first_entry, h_mode_last 125 | h_first_entry = menu_handler.insert( "First Entry" ) 126 | entry = menu_handler.insert( "deep", parent=h_first_entry) 127 | entry = menu_handler.insert( "sub", parent=entry ); 128 | entry = menu_handler.insert( "menu", parent=entry, callback=deepCb ); 129 | 130 | menu_handler.setCheckState( menu_handler.insert( "Show First Entry", callback=enableCb ), MenuHandler.CHECKED ) 131 | 132 | sub_menu_handle = menu_handler.insert( "Switch" ) 133 | for i in range(5): 134 | s = "Mode " + str(i) 135 | h_mode_last = menu_handler.insert( s, parent=sub_menu_handle, callback=modeCb ) 136 | menu_handler.setCheckState( h_mode_last, MenuHandler.UNCHECKED) 137 | # check the very last entry 138 | menu_handler.setCheckState( h_mode_last, MenuHandler.CHECKED ) 139 | 140 | if __name__=="__main__": 141 | rospy.init_node("menu") 142 | 143 | server = InteractiveMarkerServer("menu") 144 | 145 | initMenu() 146 | 147 | makeMenuMarker( "marker1" ) 148 | makeMenuMarker( "marker2" ) 149 | 150 | menu_handler.apply( server, "marker1" ) 151 | menu_handler.apply( server, "marker2" ) 152 | server.applyChanges() 153 | 154 | rospy.spin() 155 | 156 | -------------------------------------------------------------------------------- /interactive_marker_tutorials/scripts/simple_marker.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | Copyright (c) 2011, Willow Garage, Inc. 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of the Willow Garage, Inc. nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | POSSIBILITY OF SUCH DAMAGE. 30 | """ 31 | 32 | from __future__ import print_function 33 | 34 | import rospy 35 | 36 | from interactive_markers.interactive_marker_server import * 37 | from visualization_msgs.msg import * 38 | 39 | def processFeedback(feedback): 40 | p = feedback.pose.position 41 | print(feedback.marker_name + " is now at " + str(p.x) + ", " + str(p.y) + ", " + str(p.z)) 42 | 43 | if __name__=="__main__": 44 | rospy.init_node("simple_marker") 45 | 46 | # create an interactive marker server on the topic namespace simple_marker 47 | server = InteractiveMarkerServer("simple_marker") 48 | 49 | # create an interactive marker for our server 50 | int_marker = InteractiveMarker() 51 | int_marker.header.frame_id = "base_link" 52 | int_marker.name = "my_marker" 53 | int_marker.description = "Simple 1-DOF Control" 54 | 55 | # create a grey box marker 56 | box_marker = Marker() 57 | box_marker.type = Marker.CUBE 58 | box_marker.scale.x = 0.45 59 | box_marker.scale.y = 0.45 60 | box_marker.scale.z = 0.45 61 | box_marker.color.r = 0.0 62 | box_marker.color.g = 0.5 63 | box_marker.color.b = 0.5 64 | box_marker.color.a = 1.0 65 | 66 | # create a non-interactive control which contains the box 67 | box_control = InteractiveMarkerControl() 68 | box_control.always_visible = True 69 | box_control.markers.append( box_marker ) 70 | 71 | # add the control to the interactive marker 72 | int_marker.controls.append( box_control ) 73 | 74 | # create a control which will move the box 75 | # this control does not contain any markers, 76 | # which will cause RViz to insert two arrows 77 | rotate_control = InteractiveMarkerControl() 78 | rotate_control.name = "move_x" 79 | rotate_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS 80 | 81 | # add the control to the interactive marker 82 | int_marker.controls.append(rotate_control); 83 | 84 | # add the interactive marker to our collection & 85 | # tell the server to call processFeedback() when feedback arrives for it 86 | server.insert(int_marker, processFeedback) 87 | 88 | # 'commit' changes and send to all clients 89 | server.applyChanges() 90 | 91 | rospy.spin() 92 | 93 | -------------------------------------------------------------------------------- /interactive_marker_tutorials/src/cube.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2011, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | 31 | #include 32 | 33 | #include 34 | 35 | #include 36 | 37 | #include 38 | 39 | 40 | using namespace visualization_msgs; 41 | 42 | boost::shared_ptr server; 43 | 44 | std::vector< tf::Vector3 > positions; 45 | 46 | void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ) 47 | { 48 | switch ( feedback->event_type ) 49 | { 50 | case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE: 51 | { 52 | //compute difference vector for this cube 53 | 54 | tf::Vector3 fb_pos(feedback->pose.position.x, feedback->pose.position.y, feedback->pose.position.z); 55 | unsigned index = atoi( feedback->marker_name.c_str() ); 56 | 57 | if ( index > positions.size() ) 58 | { 59 | return; 60 | } 61 | tf::Vector3 fb_delta = fb_pos - positions[index]; 62 | 63 | // move all markers in that direction 64 | for ( unsigned i=0; isetPose( s.str(), pose ); 85 | } 86 | 87 | 88 | break; 89 | } 90 | } 91 | server->applyChanges(); 92 | } 93 | 94 | InteractiveMarkerControl& makeBoxControl( InteractiveMarker &msg ) 95 | { 96 | InteractiveMarkerControl control; 97 | control.always_visible = true; 98 | control.orientation_mode = InteractiveMarkerControl::VIEW_FACING; 99 | control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE; 100 | control.independent_marker_orientation = true; 101 | 102 | Marker marker; 103 | 104 | marker.type = Marker::CUBE; 105 | marker.scale.x = msg.scale; 106 | marker.scale.y = msg.scale; 107 | marker.scale.z = msg.scale; 108 | marker.color.r = 0.65+0.7*msg.pose.position.x; 109 | marker.color.g = 0.65+0.7*msg.pose.position.y; 110 | marker.color.b = 0.65+0.7*msg.pose.position.z; 111 | marker.color.a = 1.0; 112 | 113 | control.markers.push_back( marker ); 114 | msg.controls.push_back( control ); 115 | 116 | return msg.controls.back(); 117 | } 118 | 119 | 120 | void makeCube( ) 121 | { 122 | int side_length = 10; 123 | float step = 1.0/ (float)side_length; 124 | int count = 0; 125 | 126 | positions.reserve( side_length*side_length*side_length ); 127 | 128 | for ( double x=-0.5; x<0.5; x+=step ) 129 | { 130 | for ( double y=-0.5; y<0.5; y+=step ) 131 | { 132 | for ( double z=0.0; z<1.0; z+=step ) 133 | { 134 | InteractiveMarker int_marker; 135 | int_marker.header.frame_id = "base_link"; 136 | int_marker.scale = step; 137 | 138 | int_marker.pose.position.x = x; 139 | int_marker.pose.position.y = y; 140 | int_marker.pose.position.z = z; 141 | 142 | positions.push_back( tf::Vector3(x,y,z) ); 143 | 144 | std::stringstream s; 145 | s << count; 146 | int_marker.name = s.str(); 147 | 148 | makeBoxControl(int_marker); 149 | 150 | server->insert( int_marker ); 151 | server->setCallback( int_marker.name, &processFeedback ); 152 | 153 | count++; 154 | } 155 | } 156 | } 157 | } 158 | 159 | int main(int argc, char** argv) 160 | { 161 | ros::init(argc, argv, "cube"); 162 | 163 | server.reset( new interactive_markers::InteractiveMarkerServer("cube") ); 164 | 165 | ros::Duration(0.1).sleep(); 166 | 167 | ROS_INFO("initializing.."); 168 | makeCube(); 169 | server->applyChanges(); 170 | ROS_INFO("ready."); 171 | 172 | ros::spin(); 173 | 174 | server.reset(); 175 | } 176 | -------------------------------------------------------------------------------- /interactive_marker_tutorials/src/menu.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2011, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | #include 40 | 41 | using namespace visualization_msgs; 42 | using namespace interactive_markers; 43 | 44 | boost::shared_ptr server; 45 | float marker_pos = 0; 46 | 47 | MenuHandler menu_handler; 48 | 49 | MenuHandler::EntryHandle h_first_entry; 50 | MenuHandler::EntryHandle h_mode_last; 51 | 52 | 53 | void enableCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ) 54 | { 55 | MenuHandler::EntryHandle handle = feedback->menu_entry_id; 56 | MenuHandler::CheckState state; 57 | menu_handler.getCheckState( handle, state ); 58 | 59 | if ( state == MenuHandler::CHECKED ) 60 | { 61 | menu_handler.setCheckState( handle, MenuHandler::UNCHECKED ); 62 | ROS_INFO("Hiding first menu entry"); 63 | menu_handler.setVisible( h_first_entry, false ); 64 | } 65 | else 66 | { 67 | menu_handler.setCheckState( handle, MenuHandler::CHECKED ); 68 | ROS_INFO("Showing first menu entry"); 69 | menu_handler.setVisible( h_first_entry, true ); 70 | } 71 | menu_handler.reApply( *server ); 72 | ros::Duration(2.0).sleep(); 73 | ROS_INFO("update"); 74 | server->applyChanges(); 75 | } 76 | 77 | void modeCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ) 78 | { 79 | menu_handler.setCheckState( h_mode_last, MenuHandler::UNCHECKED ); 80 | h_mode_last = feedback->menu_entry_id; 81 | menu_handler.setCheckState( h_mode_last, MenuHandler::CHECKED ); 82 | 83 | ROS_INFO("Switching to menu entry #%d", h_mode_last); 84 | 85 | menu_handler.reApply( *server ); 86 | server->applyChanges(); 87 | } 88 | 89 | 90 | 91 | Marker makeBox( InteractiveMarker &msg ) 92 | { 93 | Marker marker; 94 | 95 | marker.type = Marker::CUBE; 96 | marker.scale.x = msg.scale * 0.45; 97 | marker.scale.y = msg.scale * 0.45; 98 | marker.scale.z = msg.scale * 0.45; 99 | marker.color.r = 0.5; 100 | marker.color.g = 0.5; 101 | marker.color.b = 0.5; 102 | marker.color.a = 1.0; 103 | 104 | return marker; 105 | } 106 | 107 | InteractiveMarkerControl& makeBoxControl( InteractiveMarker &msg ) 108 | { 109 | InteractiveMarkerControl control; 110 | control.always_visible = true; 111 | control.markers.push_back( makeBox(msg) ); 112 | msg.controls.push_back( control ); 113 | 114 | return msg.controls.back(); 115 | } 116 | 117 | InteractiveMarker makeEmptyMarker( bool dummyBox=true ) 118 | { 119 | InteractiveMarker int_marker; 120 | int_marker.header.frame_id = "base_link"; 121 | int_marker.pose.position.y = -3.0 * marker_pos++;; 122 | int_marker.scale = 1; 123 | 124 | return int_marker; 125 | } 126 | 127 | void makeMenuMarker( std::string name ) 128 | { 129 | InteractiveMarker int_marker = makeEmptyMarker(); 130 | int_marker.name = name; 131 | 132 | InteractiveMarkerControl control; 133 | 134 | control.interaction_mode = InteractiveMarkerControl::BUTTON; 135 | control.always_visible = true; 136 | 137 | control.markers.push_back( makeBox( int_marker ) ); 138 | int_marker.controls.push_back(control); 139 | 140 | server->insert( int_marker ); 141 | } 142 | 143 | void deepCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ) 144 | { 145 | ROS_INFO("The deep sub-menu has been found."); 146 | } 147 | 148 | void initMenu() 149 | { 150 | h_first_entry = menu_handler.insert( "First Entry" ); 151 | MenuHandler::EntryHandle entry = menu_handler.insert( h_first_entry, "deep" ); 152 | entry = menu_handler.insert( entry, "sub" ); 153 | entry = menu_handler.insert( entry, "menu", &deepCb ); 154 | 155 | menu_handler.setCheckState( menu_handler.insert( "Show First Entry", &enableCb ), MenuHandler::CHECKED ); 156 | 157 | MenuHandler::EntryHandle sub_menu_handle = menu_handler.insert( "Switch" ); 158 | 159 | for ( int i=0; i<5; i++ ) 160 | { 161 | std::ostringstream s; 162 | s << "Mode " << i; 163 | h_mode_last = menu_handler.insert( sub_menu_handle, s.str(), &modeCb ); 164 | menu_handler.setCheckState( h_mode_last, MenuHandler::UNCHECKED ); 165 | } 166 | //check the very last entry 167 | menu_handler.setCheckState( h_mode_last, MenuHandler::CHECKED ); 168 | } 169 | 170 | int main(int argc, char** argv) 171 | { 172 | ros::init(argc, argv, "menu"); 173 | 174 | server.reset( new InteractiveMarkerServer("menu","",false) ); 175 | 176 | initMenu(); 177 | 178 | makeMenuMarker( "marker1" ); 179 | makeMenuMarker( "marker2" ); 180 | 181 | menu_handler.apply( *server, "marker1" ); 182 | menu_handler.apply( *server, "marker2" ); 183 | server->applyChanges(); 184 | 185 | ros::spin(); 186 | 187 | server.reset(); 188 | } 189 | -------------------------------------------------------------------------------- /interactive_marker_tutorials/src/point_cloud.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2011, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | 31 | // %Tag(fullSource)% 32 | #include 33 | 34 | #include 35 | 36 | #include 37 | 38 | namespace vm = visualization_msgs; 39 | 40 | void processFeedback( const vm::InteractiveMarkerFeedbackConstPtr &feedback ) 41 | { 42 | uint8_t type = feedback->event_type; 43 | 44 | if( type == vm::InteractiveMarkerFeedback::BUTTON_CLICK || 45 | type == vm::InteractiveMarkerFeedback::MOUSE_DOWN || 46 | type == vm::InteractiveMarkerFeedback::MOUSE_UP ) 47 | { 48 | const char* type_str = (type == vm::InteractiveMarkerFeedback::BUTTON_CLICK ? "button click" : 49 | (type == vm::InteractiveMarkerFeedback::MOUSE_DOWN ? "mouse down" : "mouse up")); 50 | 51 | if( feedback->mouse_point_valid ) 52 | { 53 | ROS_INFO( "%s at %f, %f, %f in frame %s", 54 | type_str, 55 | feedback->mouse_point.x, feedback->mouse_point.y, feedback->mouse_point.z, 56 | feedback->header.frame_id.c_str() ); 57 | } 58 | else 59 | { 60 | ROS_INFO( "%s", type_str ); 61 | } 62 | } 63 | else if( type == vm::InteractiveMarkerFeedback::POSE_UPDATE ) 64 | { 65 | ROS_INFO_STREAM( feedback->marker_name << " is now at " 66 | << feedback->pose.position.x << ", " << feedback->pose.position.y 67 | << ", " << feedback->pose.position.z ); 68 | } 69 | } 70 | 71 | void makePoints( std::vector& points_out, int num_points ) 72 | { 73 | double radius = 3; 74 | points_out.resize(num_points); 75 | for( int i = 0; i < num_points; i++ ) 76 | { 77 | double angle = (i / (double) num_points * 50 * M_PI); 78 | double height = (i / (double) num_points * 10); 79 | points_out[i].x = radius * cos( angle ); 80 | points_out[i].y = radius * sin( angle ); 81 | points_out[i].z = height; 82 | } 83 | } 84 | 85 | vm::InteractiveMarker makeMarker( std::string name, std::string description, int32_t type, float x, int num_points = 10000, float scale = 0.1f ) 86 | { 87 | // create an interactive marker for our server 88 | vm::InteractiveMarker int_marker; 89 | int_marker.header.frame_id = "base_link"; 90 | int_marker.name = name; 91 | int_marker.description = description; 92 | 93 | // create a point cloud marker 94 | vm::Marker points_marker; 95 | points_marker.type = type; 96 | points_marker.scale.x = scale; 97 | points_marker.scale.y = scale; 98 | points_marker.scale.z = scale; 99 | points_marker.color.r = 0.5; 100 | points_marker.color.g = 0.5; 101 | points_marker.color.b = 0.5; 102 | points_marker.color.a = 1.0; 103 | makePoints( points_marker.points, num_points ); 104 | 105 | // create a control which contains the point cloud which acts like a button. 106 | vm::InteractiveMarkerControl points_control; 107 | points_control.always_visible = true; 108 | points_control.interaction_mode = vm::InteractiveMarkerControl::BUTTON; 109 | points_control.markers.push_back( points_marker ); 110 | 111 | // add the control to the interactive marker 112 | int_marker.controls.push_back( points_control ); 113 | 114 | // create a control which will move the box 115 | // this control does not contain any markers, 116 | // which will cause RViz to insert two arrows 117 | vm::InteractiveMarkerControl rotate_control; 118 | rotate_control.name = "move_x"; 119 | rotate_control.interaction_mode = 120 | vm::InteractiveMarkerControl::MOVE_AXIS; 121 | 122 | // add the control to the interactive marker 123 | int_marker.controls.push_back(rotate_control); 124 | 125 | int_marker.pose.position.x = x; 126 | 127 | return int_marker; 128 | } 129 | 130 | int main(int argc, char** argv) 131 | { 132 | ros::init(argc, argv, "point_cloud"); 133 | 134 | // create an interactive marker server on the topic namespace simple_marker 135 | interactive_markers::InteractiveMarkerServer server("point_cloud"); 136 | 137 | server.insert(makeMarker("points", "Points marker", vm::Marker::POINTS, 0), &processFeedback); 138 | // LINE_STRIP and LINE_LIST are not actually selectable, and they won't highlight or detect mouse clicks like the others (yet). 139 | server.insert(makeMarker("line_strip", "Line Strip marker", vm::Marker::LINE_STRIP, 10, 1000), &processFeedback); 140 | server.insert(makeMarker("line_list", "Line List marker", vm::Marker::LINE_LIST, 20), &processFeedback); 141 | server.insert(makeMarker("cube_list", "Cube List marker", vm::Marker::CUBE_LIST, 30), &processFeedback); 142 | server.insert(makeMarker("sphere_list", "Sphere List marker", vm::Marker::SPHERE_LIST, 40), &processFeedback); 143 | server.insert(makeMarker("triangle_list", "Triangle List marker", vm::Marker::TRIANGLE_LIST, 50, 201, 1.0f), &processFeedback); 144 | 145 | // 'commit' changes and send to all clients 146 | server.applyChanges(); 147 | 148 | // start the ROS main loop 149 | ros::spin(); 150 | } 151 | // %Tag(fullSource)% 152 | -------------------------------------------------------------------------------- /interactive_marker_tutorials/src/selection.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2011, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | 31 | // %Tag(fullSource)% 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | 41 | bool testPointAgainstAabb2(const tf::Vector3 &aabbMin1, const tf::Vector3 &aabbMax1, 42 | const tf::Vector3 &point) 43 | { 44 | bool overlap = true; 45 | overlap = (aabbMin1.getX() > point.getX() || aabbMax1.getX() < point.getX()) ? false : overlap; 46 | overlap = (aabbMin1.getZ() > point.getZ() || aabbMax1.getZ() < point.getZ()) ? false : overlap; 47 | overlap = (aabbMin1.getY() > point.getY() || aabbMax1.getY() < point.getY()) ? false : overlap; 48 | return overlap; 49 | } 50 | 51 | namespace vm = visualization_msgs; 52 | 53 | class PointCouldSelector 54 | { 55 | public: 56 | PointCouldSelector( boost::shared_ptr server, 57 | std::vector& points ) : 58 | server_( server ), 59 | min_sel_( -1, -1, -1 ), 60 | max_sel_( 1, 1, 1 ), 61 | points_( points ) 62 | { 63 | updateBox( ); 64 | updatePointClouds(); 65 | 66 | makeSizeHandles(); 67 | } 68 | 69 | void processAxisFeedback( 70 | const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ) 71 | { 72 | ROS_INFO_STREAM( feedback->marker_name << " is now at " 73 | << feedback->pose.position.x << ", " << feedback->pose.position.y 74 | << ", " << feedback->pose.position.z ); 75 | 76 | if ( feedback->marker_name == "min_x" ) min_sel_.setX( feedback->pose.position.x ); 77 | if ( feedback->marker_name == "max_x" ) max_sel_.setX( feedback->pose.position.x ); 78 | if ( feedback->marker_name == "min_y" ) min_sel_.setY( feedback->pose.position.y ); 79 | if ( feedback->marker_name == "max_y" ) max_sel_.setY( feedback->pose.position.y ); 80 | if ( feedback->marker_name == "min_z" ) min_sel_.setZ( feedback->pose.position.z ); 81 | if ( feedback->marker_name == "max_z" ) max_sel_.setZ( feedback->pose.position.z ); 82 | 83 | updateBox( ); 84 | updateSizeHandles(); 85 | 86 | if ( feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP ) 87 | { 88 | updatePointClouds(); 89 | } 90 | 91 | server_->applyChanges(); 92 | } 93 | 94 | vm::Marker makeBox( vm::InteractiveMarker &msg, 95 | tf::Vector3 min_bound, tf::Vector3 max_bound ) 96 | { 97 | vm::Marker marker; 98 | 99 | marker.type = vm::Marker::CUBE; 100 | marker.scale.x = max_bound.x() - min_bound.x(); 101 | marker.scale.y = max_bound.y() - min_bound.y(); 102 | marker.scale.z = max_bound.z() - min_bound.z(); 103 | marker.pose.position.x = 0.5 * ( max_bound.x() + min_bound.x() ); 104 | marker.pose.position.y = 0.5 * ( max_bound.y() + min_bound.y() ); 105 | marker.pose.position.z = 0.5 * ( max_bound.z() + min_bound.z() ); 106 | marker.color.r = 0.5; 107 | marker.color.g = 0.5; 108 | marker.color.b = 0.5; 109 | marker.color.a = 0.5; 110 | 111 | return marker; 112 | } 113 | 114 | void updateBox( ) 115 | { 116 | vm::InteractiveMarker msg; 117 | msg.header.frame_id = "base_link"; 118 | 119 | vm::InteractiveMarkerControl control; 120 | control.always_visible = false; 121 | control.markers.push_back( makeBox(msg, min_sel_, max_sel_) ); 122 | msg.controls.push_back( control ); 123 | 124 | server_->insert( msg ); 125 | } 126 | 127 | void updatePointCloud( std::string name, std_msgs::ColorRGBA color, std::vector &points ) 128 | { 129 | // create an interactive marker for our server 130 | vm::InteractiveMarker int_marker; 131 | int_marker.header.frame_id = "base_link"; 132 | int_marker.name = name; 133 | 134 | // create a point cloud marker 135 | vm::Marker points_marker; 136 | points_marker.type = vm::Marker::SPHERE_LIST; 137 | points_marker.scale.x = 0.05; 138 | points_marker.scale.y = 0.05; 139 | points_marker.scale.z = 0.05; 140 | points_marker.color = color; 141 | 142 | for ( unsigned i=0; iinsert( int_marker ); 161 | } 162 | 163 | void updatePointClouds() 164 | { 165 | std::vector points_in, points_out; 166 | points_in.reserve( points_.size() ); 167 | points_out.reserve( points_.size() ); 168 | 169 | // determine which points are selected (i.e. inside the selection box) 170 | for ( unsigned i=0; i0 ? "max_x" : "min_x"; 219 | int_marker.pose.position.x = sign>0 ? max_sel_.x() : min_sel_.x(); 220 | int_marker.pose.position.y = 0.5 * ( max_sel_.y() + min_sel_.y() ); 221 | int_marker.pose.position.z = 0.5 * ( max_sel_.z() + min_sel_.z() ); 222 | orien = tf::Quaternion(1.0, 0.0, 0.0, 1.0); 223 | orien.normalize(); 224 | tf::quaternionTFToMsg(orien, control.orientation); 225 | break; 226 | case 1: 227 | int_marker.name = sign>0 ? "max_y" : "min_y"; 228 | int_marker.pose.position.x = 0.5 * ( max_sel_.x() + min_sel_.x() ); 229 | int_marker.pose.position.y = sign>0 ? max_sel_.y() : min_sel_.y(); 230 | int_marker.pose.position.z = 0.5 * ( max_sel_.z() + min_sel_.z() ); 231 | orien = tf::Quaternion(0.0, 0.0, 1.0, 1.0); 232 | orien.normalize(); 233 | tf::quaternionTFToMsg(orien, control.orientation); 234 | break; 235 | default: 236 | int_marker.name = sign>0 ? "max_z" : "min_z"; 237 | int_marker.pose.position.x = 0.5 * ( max_sel_.x() + min_sel_.x() ); 238 | int_marker.pose.position.y = 0.5 * ( max_sel_.y() + min_sel_.y() ); 239 | int_marker.pose.position.z = sign>0 ? max_sel_.z() : min_sel_.z(); 240 | orien = tf::Quaternion(0.0, -1.0, 0.0, 1.0); 241 | orien.normalize(); 242 | tf::quaternionTFToMsg(orien, control.orientation); 243 | break; 244 | } 245 | 246 | interactive_markers::makeArrow( int_marker, control, 0.5 * sign ); 247 | 248 | int_marker.controls.push_back( control ); 249 | server_->insert( int_marker, [this](auto feedback){ processAxisFeedback(feedback); } ); 250 | } 251 | } 252 | } 253 | 254 | void updateSizeHandles( ) 255 | { 256 | for ( int axis=0; axis<3; axis++ ) 257 | { 258 | for ( int sign=-1; sign<=1; sign+=2 ) 259 | { 260 | std::string name; 261 | geometry_msgs::Pose pose; 262 | 263 | switch ( axis ) 264 | { 265 | case 0: 266 | name = sign>0 ? "max_x" : "min_x"; 267 | pose.position.x = sign>0 ? max_sel_.x() : min_sel_.x(); 268 | pose.position.y = 0.5 * ( max_sel_.y() + min_sel_.y() ); 269 | pose.position.z = 0.5 * ( max_sel_.z() + min_sel_.z() ); 270 | break; 271 | case 1: 272 | name = sign>0 ? "max_y" : "min_y"; 273 | pose.position.x = 0.5 * ( max_sel_.x() + min_sel_.x() ); 274 | pose.position.y = sign>0 ? max_sel_.y() : min_sel_.y(); 275 | pose.position.z = 0.5 * ( max_sel_.z() + min_sel_.z() ); 276 | break; 277 | default: 278 | name = sign>0 ? "max_z" : "min_z"; 279 | pose.position.x = 0.5 * ( max_sel_.x() + min_sel_.x() ); 280 | pose.position.y = 0.5 * ( max_sel_.y() + min_sel_.y() ); 281 | pose.position.z = sign>0 ? max_sel_.z() : min_sel_.z(); 282 | break; 283 | } 284 | 285 | server_->setPose( name, pose ); 286 | } 287 | } 288 | } 289 | 290 | private: 291 | boost::shared_ptr server_; 292 | 293 | tf::Vector3 min_sel_, max_sel_; 294 | std::vector points_; 295 | 296 | vm::InteractiveMarker sel_points_marker_; 297 | vm::InteractiveMarker unsel_points_marker_; 298 | }; 299 | 300 | 301 | 302 | 303 | double rand( double min, double max ) 304 | { 305 | double t = (double)rand() / (double)RAND_MAX; 306 | return min + t*(max-min); 307 | } 308 | 309 | 310 | void makePoints( std::vector& points_out, int num_points ) 311 | { 312 | double radius = 3; 313 | double scale = 0.2; 314 | points_out.resize(num_points); 315 | for( int i = 0; i < num_points; i++ ) 316 | { 317 | points_out[i].setX( scale * rand( -radius, radius ) ); 318 | points_out[i].setY( scale * rand( -radius, radius ) ); 319 | points_out[i].setZ( scale * radius * 0.2 * ( sin( 10.0 / radius * points_out[i].x() ) + cos( 10.0 / radius * points_out[i].y() ) ) ); 320 | } 321 | } 322 | 323 | 324 | int main(int argc, char** argv) 325 | { 326 | ros::init(argc, argv, "selection"); 327 | 328 | // create an interactive marker server on the topic namespace simple_marker 329 | boost::shared_ptr server( 330 | new interactive_markers::InteractiveMarkerServer("selection") ); 331 | 332 | std::vector points; 333 | makePoints( points, 10000 ); 334 | 335 | PointCouldSelector selector( server, points ); 336 | 337 | // 'commit' changes and send to all clients 338 | server->applyChanges(); 339 | 340 | // start the ROS main loop 341 | ros::spin(); 342 | } 343 | // %Tag(fullSource)% 344 | -------------------------------------------------------------------------------- /interactive_marker_tutorials/src/simple_marker.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2011, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | 31 | // %Tag(fullSource)% 32 | #include 33 | 34 | #include 35 | 36 | void processFeedback( 37 | const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ) 38 | { 39 | ROS_INFO_STREAM( feedback->marker_name << " is now at " 40 | << feedback->pose.position.x << ", " << feedback->pose.position.y 41 | << ", " << feedback->pose.position.z ); 42 | } 43 | 44 | int main(int argc, char** argv) 45 | { 46 | ros::init(argc, argv, "simple_marker"); 47 | 48 | // create an interactive marker server on the topic namespace simple_marker 49 | interactive_markers::InteractiveMarkerServer server("simple_marker"); 50 | 51 | // create an interactive marker for our server 52 | visualization_msgs::InteractiveMarker int_marker; 53 | int_marker.header.frame_id = "base_link"; 54 | int_marker.header.stamp=ros::Time::now(); 55 | int_marker.name = "my_marker"; 56 | int_marker.description = "Simple 1-DOF Control"; 57 | 58 | // create a grey box marker 59 | visualization_msgs::Marker box_marker; 60 | box_marker.type = visualization_msgs::Marker::CUBE; 61 | box_marker.scale.x = 0.45; 62 | box_marker.scale.y = 0.45; 63 | box_marker.scale.z = 0.45; 64 | box_marker.color.r = 0.5; 65 | box_marker.color.g = 0.5; 66 | box_marker.color.b = 0.5; 67 | box_marker.color.a = 1.0; 68 | 69 | // create a non-interactive control which contains the box 70 | visualization_msgs::InteractiveMarkerControl box_control; 71 | box_control.always_visible = true; 72 | box_control.markers.push_back( box_marker ); 73 | 74 | // add the control to the interactive marker 75 | int_marker.controls.push_back( box_control ); 76 | 77 | // create a control which will move the box 78 | // this control does not contain any markers, 79 | // which will cause RViz to insert two arrows 80 | visualization_msgs::InteractiveMarkerControl rotate_control; 81 | rotate_control.name = "move_x"; 82 | rotate_control.interaction_mode = 83 | visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; 84 | 85 | // add the control to the interactive marker 86 | int_marker.controls.push_back(rotate_control); 87 | 88 | // add the interactive marker to our collection & 89 | // tell the server to call processFeedback() when feedback arrives for it 90 | server.insert(int_marker, &processFeedback); 91 | 92 | // 'commit' changes and send to all clients 93 | server.applyChanges(); 94 | 95 | // start the ROS main loop 96 | ros::spin(); 97 | } 98 | // %Tag(fullSource)% 99 | -------------------------------------------------------------------------------- /librviz_tutorial/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package librviz_tutorial 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.11.2 (2025-04-26) 6 | ------------------- 7 | * Eliminate Python 2 code (`#88 `_) 8 | * Contributors: vineet131 9 | 10 | 0.11.1 (2025-04-10) 11 | ------------------- 12 | * Update maintainers (`#63 `_) 13 | * Contributors: Mabel Zhang 14 | 15 | 0.11.0 (2020-05-13) 16 | ------------------- 17 | 18 | 0.10.4 (2020-05-13) 19 | ------------------- 20 | * Updated required CMake version to avoid CMP0048 warning (`#57 `_) 21 | * Contributors: Alejandro Hernández Cordero 22 | 23 | 0.10.3 (2018-05-09) 24 | ------------------- 25 | 26 | 0.10.2 (2018-01-05) 27 | ------------------- 28 | * Unified find_package for Qt4 and Qt5. (`#33 `_) 29 | * Contributors: Robert Haschke, William Woodall 30 | 31 | 0.10.1 (2016-04-21) 32 | ------------------- 33 | * Added qt5 dependencies to the package.xml. 34 | * Contributors: William Woodall 35 | 36 | 0.10.0 (2016-04-21) 37 | ------------------- 38 | * Added support Qt5 in Kinetic. 39 | * Contributors: William Woodall 40 | 41 | 0.9.2 (2015-09-21) 42 | ------------------ 43 | 44 | 0.9.1 (2015-01-26) 45 | ------------------ 46 | * Renamed a CMake variable to avoid colliding with built-in name. 47 | * librviz_tutorial now installs it's executable ``myviz``. 48 | * Removed explicit default_plugin library to fix "ld: cannot find -ldefault_plugin" isolated build error 49 | * Contributors: Honore Doktorr, Kei Okada, William Woodall 50 | 51 | 0.9.0 (2014-03-24) 52 | ------------------ 53 | * set myself (william) as maintainer 54 | * Contributors: William Woodall 55 | -------------------------------------------------------------------------------- /librviz_tutorial/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ## BEGIN_TUTORIAL 2 | ## This CMakeLists.txt file for rviz_plugin_tutorials builds both the 3 | ## TeleopPanel tutorial and the ImuDisplay tutorial. 4 | ## 5 | ## First start with some standard catkin stuff. 6 | cmake_minimum_required(VERSION 3.0.2) 7 | project(librviz_tutorial) 8 | find_package(catkin REQUIRED COMPONENTS rviz roscpp) 9 | catkin_package() 10 | include_directories(${catkin_INCLUDE_DIRS}) 11 | link_directories(${catkin_LIBRARY_DIRS}) 12 | 13 | ## This setting causes Qt's "MOC" generation to happen automatically. 14 | set(CMAKE_AUTOMOC ON) 15 | 16 | ## This plugin includes Qt widgets, so we must include Qt. 17 | ## We'll use the version that rviz used so they are compatible. 18 | if(rviz_QT_VERSION VERSION_LESS "5") 19 | message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 20 | find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) 21 | ## pull in all required include dirs, define QT_LIBRARIES, etc. 22 | include(${QT_USE_FILE}) 23 | else() 24 | message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 25 | find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) 26 | ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies 27 | set(QT_LIBRARIES Qt5::Widgets) 28 | endif() 29 | 30 | ## I prefer the Qt signals and slots to avoid defining "emit", "slots", 31 | ## etc because they can conflict with boost signals, so define QT_NO_KEYWORDS here. 32 | add_definitions(-DQT_NO_KEYWORDS) 33 | 34 | ## Here we specify the list of source files. 35 | ## The generated MOC files are included automatically as headers. 36 | set(SRC_FILES 37 | src/myviz.cpp 38 | src/main.cpp 39 | ) 40 | 41 | ## Add the "myviz" executable and specify the list of source files we 42 | ## collected above in ``${SRC_FILES}``. 43 | add_executable(myviz ${SRC_FILES}) 44 | 45 | ## Link the myviz executable with whatever Qt libraries have been defined by 46 | ## the ``find_package(Qt4 ...)`` line above, or by the 47 | ## ``set(QT_LIBRARIES Qt5::Widgets)``, and with whatever libraries 48 | ## catkin has included. 49 | target_link_libraries(myviz ${QT_LIBRARIES} ${catkin_LIBRARIES}) 50 | ## END_TUTORIAL 51 | 52 | ## Install 53 | install(TARGETS myviz DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 54 | -------------------------------------------------------------------------------- /librviz_tutorial/package.xml: -------------------------------------------------------------------------------- 1 | 2 | librviz_tutorial 3 | 0.11.2 4 | 5 | Tutorial showing how to compile your own C++ program with RViz displays and features. 6 | 7 | Mabel Zhang 8 | BSD 9 | 10 | http://ros.org/wiki/librviz_tutorial 11 | Dave Hershberger 12 | 13 | catkin 14 | 15 | qtbase5-dev 16 | roscpp 17 | rviz 18 | 19 | libqt5-core 20 | libqt5-gui 21 | libqt5-widgets 22 | roscpp 23 | rviz 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /librviz_tutorial/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: sphinx 2 | sphinx_root_dir: src/doc 3 | -------------------------------------------------------------------------------- /librviz_tutorial/src/doc/conf.py: -------------------------------------------------------------------------------- 1 | import sys, os 2 | 3 | sys.path += [ os.path.abspath( '.' )] 4 | 5 | extensions = [ 'sphinx.ext.extlinks', 6 | 'tutorialformatter' ] 7 | 8 | # The master toctree document. 9 | master_doc = 'index' 10 | 11 | # The suffix of source filenames. 12 | source_suffix = '.rst' 13 | 14 | project = u'librviz_tutorial' 15 | 16 | copyright = u'2012, Willow Garage, Inc' 17 | 18 | # If true, sectionauthor and moduleauthor directives will be shown in the 19 | # output. They are ignored by default. 20 | show_authors = True 21 | 22 | # The name of the Pygments (syntax highlighting) style to use. 23 | pygments_style = 'sphinx' 24 | 25 | extlinks = {'codedir': ('https://github.com/ros-visualization/visualization_tutorials/blob/hydro-devel/librviz_tutorial/%s', '')} 26 | -------------------------------------------------------------------------------- /librviz_tutorial/src/doc/index.rst: -------------------------------------------------------------------------------- 1 | Librviz Tutorial 2 | ================ 3 | 4 | Overview 5 | -------- 6 | 7 | RViz is not just a visualizer application, it is also a library! Much 8 | of RViz's functionality can be accessed within your own application by 9 | linking against librviz.so (or whatever your OS likes to call it). 10 | 11 | This tutorial shows a very simple example of creating a 3D visualizer 12 | widget (rviz::RenderPanel), programmatically creating a new Grid 13 | display within it, then using Qt slider controls to adjust a couple of 14 | the grid's properties. The app is called "myviz". 15 | 16 | The source code for this tutorial is in the librviz_tutorial 17 | package. You can check out the source directly or (if you use Ubuntu) 18 | you can just apt-get install the pre-compiled Debian package like so:: 19 | 20 | sudo apt-get install ros-hydro-visualization-tutorials 21 | 22 | The running application looks like this: 23 | 24 | .. image:: myviz.png 25 | 26 | The Code 27 | -------- 28 | 29 | The code for myviz is in these files: 30 | :codedir:`src/main.cpp`, 31 | :codedir:`src/myviz.h`, and 32 | :codedir:`src/myviz.cpp`. 33 | 34 | main.cpp 35 | ^^^^^^^^ 36 | 37 | The full text of main.cpp is here: :codedir:`src/main.cpp` 38 | 39 | .. tutorial-formatter:: ../main.cpp 40 | 41 | myviz.h 42 | ^^^^^^^ 43 | 44 | The full text of myviz.h is here: :codedir:`src/myviz.h` 45 | 46 | .. tutorial-formatter:: ../myviz.h 47 | 48 | myviz.cpp 49 | ^^^^^^^^^ 50 | 51 | The full text of myviz.cpp is here: :codedir:`src/myviz.cpp` 52 | 53 | .. tutorial-formatter:: ../myviz.cpp 54 | 55 | Building 56 | -------- 57 | 58 | The full text of CMakeLists.txt is here: :codedir:`CMakeLists.txt` 59 | 60 | .. tutorial-formatter:: ../../CMakeLists.txt 61 | 62 | Running 63 | ------- 64 | 65 | Just type:: 66 | 67 | rosrun librviz_tutorial myviz 68 | -------------------------------------------------------------------------------- /librviz_tutorial/src/doc/myviz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-visualization/visualization_tutorials/3141f0dfcb7c34ce57909226d5394ca5fd57a5c7/librviz_tutorial/src/doc/myviz.png -------------------------------------------------------------------------------- /librviz_tutorial/src/doc/tutorialformatter.py: -------------------------------------------------------------------------------- 1 | """ 2 | tutorialformatter 3 | =========================== 4 | 5 | This extension provides a directive to include a source code file 6 | in a document, but with certain comments from the file formatted 7 | as regular document text. This allows code for a tutorial to look like: 8 | 9 | /// BEGIN_TUTORIAL 10 | /// This next line adds one. 11 | i = i + 1; 12 | /// Then we need to double it. 13 | i = i * 2; 14 | /// END_TUTORIAL 15 | 16 | And have it formatted as 17 | 18 | This next line adds one.:: 19 | i = i + 1; 20 | 21 | Then we need to double it.:: 22 | i = i * 2; 23 | 24 | The special-looking comment character sequence at the start of 25 | each text line can be anything not starting or ending with 26 | whitespace. tutorialformatter starts by scanning the file for the 27 | string BEGIN_TUTORIAL. When it finds it, it takes all the 28 | characters before BEGIN_TUTORIAL on that line, strips whitespace 29 | from the left, and uses that as the text marker. So this would 30 | also be fine: 31 | 32 | #My Tutorial# BEGIN_TUTORIAL 33 | #My Tutorial# This next line adds one. 34 | i = i + 1 35 | #My Tutorial# Then we need to double it. 36 | i = i * 2 37 | #My Tutorial# END_TUTORIAL 38 | 39 | .. moduleauthor:: Dave Hershberger 40 | """ 41 | 42 | __version__ = '0.1.0' 43 | 44 | import os 45 | from docutils.parsers import rst 46 | from docutils.parsers.rst.directives import flag, unchanged 47 | from docutils.statemachine import string2lines 48 | from pygments.lexers import get_lexer_for_filename 49 | 50 | class TutorialFormatterDirective(rst.Directive): 51 | has_content = False 52 | final_argument_whitespace = True 53 | required_arguments = 1 54 | 55 | option_spec = dict(shell=flag, prompt=flag, nostderr=flag, 56 | in_srcdir=flag, extraargs=unchanged, 57 | until=unchanged) 58 | 59 | def run(self): 60 | filename = self.arguments[0] 61 | text_tag = None 62 | tag_len = 0 63 | 64 | filepath = self.state.document.settings.env.srcdir 65 | absfilename = os.path.join( filepath, filename ) 66 | if absfilename.endswith('.h'): 67 | language = 'c++' 68 | elif absfilename.endswith('CMakeLists.txt'): 69 | language = 'cmake' 70 | else: 71 | try: 72 | language = get_lexer_for_filename( absfilename ).name.lower() 73 | if language == 'text only': 74 | language = 'none' 75 | except: 76 | language = 'none' 77 | code_prefix = '\n.. code-block:: ' + language + '\n\n' 78 | code_suffix = '\n' 79 | 80 | print(f"tutorial-formatter running on {absfilename}") 81 | file_ = open( absfilename, 'r' ) 82 | text_to_process = "" 83 | current_block = "" 84 | in_code = False 85 | in_text = False 86 | in_tutorial = False 87 | for line in file_: 88 | if not in_tutorial: 89 | begin_pos = line.find( 'BEGIN_TUTORIAL' ) 90 | if begin_pos != -1: 91 | text_tag = line[:begin_pos].lstrip() 92 | tag_len = len( text_tag ) 93 | in_tutorial = True 94 | continue 95 | if line.find( 'END_TUTORIAL' ) != -1: 96 | break 97 | stripped = line.lstrip() 98 | if stripped.startswith( text_tag.strip() ): 99 | if in_code: 100 | text_to_process += code_prefix + current_block + code_suffix 101 | current_block = "" 102 | in_code = False 103 | in_text = True 104 | addition = stripped[tag_len:] 105 | if addition == '' or addition[-1] != '\n': 106 | addition += '\n' 107 | current_block += addition 108 | else: 109 | if in_text: 110 | text_to_process += current_block 111 | current_block = "" 112 | in_text = False 113 | in_code = True # Code to show begins right after tagged text 114 | if in_code: 115 | current_block += ' ' + line 116 | if in_code: 117 | text_to_process += code_prefix + current_block + code_suffix 118 | elif in_text: 119 | text_to_process += current_block 120 | 121 | # Debug writes... 122 | #print 'text_to_process =' 123 | #print text_to_process 124 | #print '= text_to_process' 125 | 126 | lines = string2lines( text_to_process ) 127 | self.state_machine.insert_input( lines, absfilename ) 128 | 129 | return [] 130 | 131 | def setup(app): 132 | app.add_directive('tutorial-formatter', TutorialFormatterDirective) 133 | -------------------------------------------------------------------------------- /librviz_tutorial/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | // BEGIN_TUTORIAL 31 | 32 | // The main() for this "myviz" example is very simple, it just 33 | // initializes ROS, creates a QApplication, creates the top-level 34 | // widget (of type "MyViz"), shows it, and runs the Qt event loop. 35 | 36 | #include 37 | #include 38 | #include "myviz.h" 39 | 40 | int main(int argc, char **argv) 41 | { 42 | if( !ros::isInitialized() ) 43 | { 44 | ros::init( argc, argv, "myviz", ros::init_options::AnonymousName ); 45 | } 46 | 47 | QApplication app( argc, argv ); 48 | 49 | MyViz* myviz = new MyViz(); 50 | myviz->show(); 51 | 52 | app.exec(); 53 | 54 | delete myviz; 55 | } 56 | -------------------------------------------------------------------------------- /librviz_tutorial/src/myviz.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include "rviz/visualization_manager.h" 37 | #include "rviz/render_panel.h" 38 | #include "rviz/display.h" 39 | 40 | #include "myviz.h" 41 | 42 | // BEGIN_TUTORIAL 43 | // Constructor for MyViz. This does most of the work of the class. 44 | MyViz::MyViz( QWidget* parent ) 45 | : QWidget( parent ) 46 | { 47 | // Construct and lay out labels and slider controls. 48 | QLabel* thickness_label = new QLabel( "Line Thickness" ); 49 | QSlider* thickness_slider = new QSlider( Qt::Horizontal ); 50 | thickness_slider->setMinimum( 1 ); 51 | thickness_slider->setMaximum( 100 ); 52 | QLabel* cell_size_label = new QLabel( "Cell Size" ); 53 | QSlider* cell_size_slider = new QSlider( Qt::Horizontal ); 54 | cell_size_slider->setMinimum( 1 ); 55 | cell_size_slider->setMaximum( 100 ); 56 | QGridLayout* controls_layout = new QGridLayout(); 57 | controls_layout->addWidget( thickness_label, 0, 0 ); 58 | controls_layout->addWidget( thickness_slider, 0, 1 ); 59 | controls_layout->addWidget( cell_size_label, 1, 0 ); 60 | controls_layout->addWidget( cell_size_slider, 1, 1 ); 61 | 62 | // Construct and lay out render panel. 63 | render_panel_ = new rviz::RenderPanel(); 64 | QVBoxLayout* main_layout = new QVBoxLayout; 65 | main_layout->addLayout( controls_layout ); 66 | main_layout->addWidget( render_panel_ ); 67 | 68 | // Set the top-level layout for this MyViz widget. 69 | setLayout( main_layout ); 70 | 71 | // Make signal/slot connections. 72 | connect( thickness_slider, SIGNAL( valueChanged( int )), this, SLOT( setThickness( int ))); 73 | connect( cell_size_slider, SIGNAL( valueChanged( int )), this, SLOT( setCellSize( int ))); 74 | 75 | // Next we initialize the main RViz classes. 76 | // 77 | // The VisualizationManager is the container for Display objects, 78 | // holds the main Ogre scene, holds the ViewController, etc. It is 79 | // very central and we will probably need one in every usage of 80 | // librviz. 81 | manager_ = new rviz::VisualizationManager( render_panel_ ); 82 | render_panel_->initialize( manager_->getSceneManager(), manager_ ); 83 | manager_->initialize(); 84 | manager_->startUpdate(); 85 | 86 | // Create a Grid display. 87 | grid_ = manager_->createDisplay( "rviz/Grid", "adjustable grid", true ); 88 | ROS_ASSERT( grid_ != NULL ); 89 | 90 | // Configure the GridDisplay the way we like it. 91 | grid_->subProp( "Line Style" )->setValue( "Billboards" ); 92 | grid_->subProp( "Color" )->setValue( QColor( Qt::yellow ) ); 93 | 94 | // Initialize the slider values. 95 | thickness_slider->setValue( 25 ); 96 | cell_size_slider->setValue( 10 ); 97 | } 98 | 99 | // Destructor. 100 | MyViz::~MyViz() 101 | { 102 | delete manager_; 103 | } 104 | 105 | // This function is a Qt slot connected to a QSlider's valueChanged() 106 | // signal. It sets the line thickness of the grid by changing the 107 | // grid's "Line Width" property. 108 | void MyViz::setThickness( int thickness_percent ) 109 | { 110 | if( grid_ != NULL ) 111 | { 112 | grid_->subProp( "Line Style" )->subProp( "Line Width" )->setValue( thickness_percent / 100.0f ); 113 | } 114 | } 115 | 116 | // This function is a Qt slot connected to a QSlider's valueChanged() 117 | // signal. It sets the cell size of the grid by changing the grid's 118 | // "Cell Size" Property. 119 | void MyViz::setCellSize( int cell_size_percent ) 120 | { 121 | if( grid_ != NULL ) 122 | { 123 | grid_->subProp( "Cell Size" )->setValue( cell_size_percent / 10.0f ); 124 | } 125 | } 126 | -------------------------------------------------------------------------------- /librviz_tutorial/src/myviz.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | #ifndef MYVIZ_H 30 | #define MYVIZ_H 31 | 32 | #include 33 | 34 | namespace rviz 35 | { 36 | class Display; 37 | class RenderPanel; 38 | class VisualizationManager; 39 | } 40 | 41 | // BEGIN_TUTORIAL 42 | // Class "MyViz" implements the top level widget for this example. 43 | class MyViz: public QWidget 44 | { 45 | Q_OBJECT 46 | public: 47 | MyViz( QWidget* parent = 0 ); 48 | virtual ~MyViz(); 49 | 50 | private Q_SLOTS: 51 | void setThickness( int thickness_percent ); 52 | void setCellSize( int cell_size_percent ); 53 | 54 | private: 55 | rviz::VisualizationManager* manager_; 56 | rviz::RenderPanel* render_panel_; 57 | rviz::Display* grid_; 58 | }; 59 | // END_TUTORIAL 60 | #endif // MYVIZ_H 61 | -------------------------------------------------------------------------------- /rviz_plugin_tutorials/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rviz_plugin_tutorials 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.11.2 (2025-04-26) 6 | ------------------- 7 | * Eliminate Python 2 code (`#88 `_) 8 | * Include class_list_macros.hpp instead of h (`#91 `_) 9 | * Do not forward-declare Ogre types (`#84 `_) 10 | * Contributors: Lucas Walter, Michael Görner, Robert Haschke, vineet131 11 | 12 | 0.11.1 (2025-04-10) 13 | ------------------- 14 | * Update maintainers (`#63 `_) 15 | * Contributors: Mabel Zhang 16 | 17 | 0.11.0 (2020-05-13) 18 | ------------------- 19 | 20 | 0.10.4 (2020-05-13) 21 | ------------------- 22 | * Updated to use ``catkin_install_python()`` (`#59 `_) 23 | * Updated required CMake version to avoid CMP0048 warning (`#57 `_) 24 | * Contributors: Alejandro Hernández Cordero, Shane Loretz 25 | 26 | 0.10.3 (2018-05-09) 27 | ------------------- 28 | * Fixed a warning due to a publisher which did not use the keyword argument 'queue_size' (`#43 `_) 29 | * Changed manifest.xml to package.xml in documentation (`#42 `_) 30 | * Contributors: Zihan Chen 31 | 32 | 0.10.2 (2018-01-05) 33 | ------------------- 34 | * Unified find_package for Qt4 and Qt5. (`#33 `_) 35 | * Contributors: Robert Haschke, William Woodall 36 | 37 | 0.10.1 (2016-04-21) 38 | ------------------- 39 | * Added qt5 dependencies to the package.xml. 40 | * Contributors: William Woodall 41 | 42 | 0.10.0 (2016-04-21) 43 | ------------------- 44 | * Added support Qt5 in Kinetic. 45 | * Contributors: William Woodall 46 | 47 | 0.9.2 (2015-09-21) 48 | ------------------ 49 | 50 | 0.9.1 (2015-01-26) 51 | ------------------ 52 | * Added ``#ifndef Q_MOC_RUN`` guard for compatibility with newer boost versions. 53 | * Contributors: Ryohei Ueda, William Woodall 54 | 55 | 0.9.0 (2014-03-24) 56 | ------------------ 57 | * set myself (william) as maintainer 58 | * Contributors: William Woodall 59 | -------------------------------------------------------------------------------- /rviz_plugin_tutorials/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ## BEGIN_TUTORIAL 2 | ## This CMakeLists.txt file for rviz_plugin_tutorials builds both the 3 | ## TeleopPanel tutorial and the ImuDisplay tutorial. 4 | ## 5 | ## First start with some standard catkin stuff. 6 | cmake_minimum_required(VERSION 3.0.2) 7 | project(rviz_plugin_tutorials) 8 | find_package(catkin REQUIRED COMPONENTS rviz) 9 | catkin_package() 10 | include_directories(${catkin_INCLUDE_DIRS}) 11 | link_directories(${catkin_LIBRARY_DIRS}) 12 | 13 | ## This setting causes Qt's "MOC" generation to happen automatically. 14 | set(CMAKE_AUTOMOC ON) 15 | 16 | ## This plugin includes Qt widgets, so we must include Qt. 17 | ## We'll use the version that rviz used so they are compatible. 18 | if(rviz_QT_VERSION VERSION_LESS "5") 19 | message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 20 | find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) 21 | ## pull in all required include dirs, define QT_LIBRARIES, etc. 22 | include(${QT_USE_FILE}) 23 | else() 24 | message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 25 | find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) 26 | ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies 27 | set(QT_LIBRARIES Qt5::Widgets) 28 | endif() 29 | 30 | ## I prefer the Qt signals and slots to avoid defining "emit", "slots", 31 | ## etc because they can conflict with boost signals, so define QT_NO_KEYWORDS here. 32 | add_definitions(-DQT_NO_KEYWORDS) 33 | 34 | ## Here we specify the list of source files. 35 | ## The generated MOC files are included automatically as headers. 36 | set(SRC_FILES 37 | src/drive_widget.cpp 38 | src/imu_display.cpp 39 | src/imu_visual.cpp 40 | src/plant_flag_tool.cpp 41 | src/teleop_panel.cpp 42 | ) 43 | 44 | ## An rviz plugin is just a shared library, so here we declare the 45 | ## library to be called ``${PROJECT_NAME}`` (which is 46 | ## "rviz_plugin_tutorials", or whatever your version of this project 47 | ## is called) and specify the list of source files we collected above 48 | ## in ``${SRC_FILES}``. 49 | add_library(${PROJECT_NAME} ${SRC_FILES}) 50 | 51 | ## Link the myviz executable with whatever Qt libraries have been defined by 52 | ## the ``find_package(Qt4 ...)`` line above, or by the 53 | ## ``set(QT_LIBRARIES Qt5::Widgets)``, and with whatever libraries 54 | ## catkin has included. 55 | ## 56 | ## Although this puts "rviz_plugin_tutorials" (or whatever you have 57 | ## called the project) as the name of the library, cmake knows it is a 58 | ## library and names the actual file something like 59 | ## "librviz_plugin_tutorials.so", or whatever is appropriate for your 60 | ## particular OS. 61 | target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES}) 62 | ## END_TUTORIAL 63 | 64 | ## Install rules 65 | 66 | install(TARGETS 67 | ${PROJECT_NAME} 68 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 69 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 70 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 71 | ) 72 | 73 | install(FILES 74 | plugin_description.xml 75 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 76 | 77 | install(DIRECTORY media/ 78 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/media) 79 | 80 | install(DIRECTORY icons/ 81 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/icons) 82 | 83 | catkin_install_python(PROGRAMS scripts/send_test_msgs.py 84 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 85 | -------------------------------------------------------------------------------- /rviz_plugin_tutorials/icons/classes/Imu.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-visualization/visualization_tutorials/3141f0dfcb7c34ce57909226d5394ca5fd57a5c7/rviz_plugin_tutorials/icons/classes/Imu.png -------------------------------------------------------------------------------- /rviz_plugin_tutorials/icons/classes/PlantFlag.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-visualization/visualization_tutorials/3141f0dfcb7c34ce57909226d5394ca5fd57a5c7/rviz_plugin_tutorials/icons/classes/PlantFlag.png -------------------------------------------------------------------------------- /rviz_plugin_tutorials/icons/classes/Teleop.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-visualization/visualization_tutorials/3141f0dfcb7c34ce57909226d5394ca5fd57a5c7/rviz_plugin_tutorials/icons/classes/Teleop.png -------------------------------------------------------------------------------- /rviz_plugin_tutorials/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rviz_plugin_tutorials 3 | 0.11.2 4 | 5 | Tutorials showing how to write plugins for RViz. 6 | 7 | Mabel Zhang 8 | BSD 9 | 10 | http://ros.org/wiki/rviz_plugin_tutorials 11 | Dave Hershberger 12 | 13 | catkin 14 | 15 | qtbase5-dev 16 | rviz 17 | 18 | libqt5-core 19 | libqt5-gui 20 | libqt5-widgets 21 | rviz 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /rviz_plugin_tutorials/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | A panel widget allowing simple diff-drive style robot base control. 7 | 8 | 9 | 12 | 13 | Displays direction and scale of accelerations from sensor_msgs/Imu messages. 14 | 15 | sensor_msgs/Imu 16 | 17 | 20 | 21 | Tool for planting flags on the ground plane in rviz. 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /rviz_plugin_tutorials/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: sphinx 2 | sphinx_root_dir: src/doc 3 | -------------------------------------------------------------------------------- /rviz_plugin_tutorials/scripts/send_test_msgs.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import roslib; roslib.load_manifest( 'rviz_plugin_tutorials' ) 4 | from sensor_msgs.msg import Imu 5 | import rospy 6 | from math import cos, sin 7 | import tf 8 | 9 | topic = 'test_imu' 10 | publisher = rospy.Publisher( topic, Imu, queue_size=5 ) 11 | 12 | rospy.init_node( 'test_imu' ) 13 | 14 | br = tf.TransformBroadcaster() 15 | rate = rospy.Rate(10) 16 | radius = 5 17 | angle = 0 18 | 19 | dist = 3 20 | while not rospy.is_shutdown(): 21 | 22 | imu = Imu() 23 | imu.header.frame_id = "/base_link" 24 | imu.header.stamp = rospy.Time.now() 25 | 26 | imu.linear_acceleration.x = sin( 10 * angle ) 27 | imu.linear_acceleration.y = sin( 20 * angle ) 28 | imu.linear_acceleration.z = sin( 40 * angle ) 29 | 30 | publisher.publish( imu ) 31 | 32 | br.sendTransform((radius * cos(angle), radius * sin(angle), 0), 33 | tf.transformations.quaternion_from_euler(0, 0, angle), 34 | rospy.Time.now(), 35 | "base_link", 36 | "map") 37 | angle += .01 38 | rate.sleep() 39 | 40 | -------------------------------------------------------------------------------- /rviz_plugin_tutorials/src/doc/building_and_exporting.rst: -------------------------------------------------------------------------------- 1 | Building the Plugin 2 | ------------------- 3 | 4 | .. tutorial-formatter:: ../../CMakeLists.txt 5 | 6 | To build the plugin, just do the normal "rosmake" thing:: 7 | 8 | rosmake rviz_plugin_tutorials 9 | 10 | Exporting the Plugin 11 | -------------------- 12 | 13 | For the plugin to be found and understood by other ROS packages (in 14 | this case, rviz), it needs a "plugin_description.xml" file. This file 15 | can be named anything you like, as it is specified in the plugin 16 | package's "package.xml" file like so: 17 | 18 | .. code-block:: xml 19 | 20 | 21 | 22 | 23 | 24 | The contents of plugin_description.xml then look like this: 25 | 26 | .. code-block:: xml 27 | 28 | 29 | 32 | 33 | A panel widget allowing simple diff-drive style robot base control. 34 | 35 | 36 | 39 | 40 | Displays direction and scale of accelerations from sensor_msgs/Imu messages. 41 | 42 | sensor_msgs/Imu 43 | 44 | 47 | 48 | Tool for planting flags on the ground plane in rviz. 49 | 50 | 51 | 52 | 53 | The first line says that the compiled library lives in 54 | lib/librviz_plugin_tutorials (the ".so" ending is appended by 55 | pluginlib according to the OS). This path is relative to the top 56 | directory of the package: 57 | 58 | .. code-block:: xml 59 | 60 | 61 | 62 | The next section is a ``class`` entry describing the TeleopPanel: 63 | 64 | .. code-block:: xml 65 | 66 | 69 | 70 | A panel widget allowing simple diff-drive style robot base control. 71 | 72 | 73 | 74 | This specifies the name, type, base class, and description of the 75 | class. The *name* field must be a combination of the first two 76 | strings given to the ``PLUGINLIB_DECLARE_CLASS()`` macro in the source 77 | file. It must be the "package" name, a "/" slash, then the "display 78 | name" for the class. The "display name" is the name used for the 79 | class in the user interface. 80 | 81 | The *type* entry must be the fully-qualified class name, including any 82 | namespace(s) it is inside. 83 | 84 | The *base_class_type* is usually one of ``rviz::Panel``, 85 | ``rviz::Display``, ``rviz::Tool``, or ``rviz::ViewController``. 86 | 87 | The *description* subsection is a simple text description of the 88 | class, which is shown in the class-chooser dialog and in the Displays 89 | panel help area. This section can contain HTML, including hyperlinks, 90 | but the markup must be escaped to avoid being interpreted as XML 91 | markup. For example a link tag might look like: ``<a 92 | href="my-web-page.html">``. 93 | 94 | Display plugins can have multiple *message_type* tags, which are used 95 | by RViz when you add a Display by selecting it's topic first. 96 | 97 | -------------------------------------------------------------------------------- /rviz_plugin_tutorials/src/doc/conf.py: -------------------------------------------------------------------------------- 1 | import sys, os 2 | 3 | sys.path += [ os.path.abspath( '.' )] 4 | 5 | extensions = [ 'sphinx.ext.extlinks', 6 | 'tutorialformatter' ] 7 | 8 | # The master toctree document. 9 | master_doc = 'index' 10 | 11 | # The suffix of source filenames. 12 | source_suffix = '.rst' 13 | 14 | project = u'rviz_plugin_tutorials' 15 | 16 | copyright = u'2012, Willow Garage, Inc' 17 | 18 | # If true, sectionauthor and moduleauthor directives will be shown in the 19 | # output. They are ignored by default. 20 | show_authors = True 21 | 22 | # The name of the Pygments (syntax highlighting) style to use. 23 | pygments_style = 'sphinx' 24 | 25 | extlinks = {'srcdir': ('https://github.com/ros-visualization/visualization_tutorials/tree/groovy-devel/rviz_plugin_tutorials/%s', '')} 26 | -------------------------------------------------------------------------------- /rviz_plugin_tutorials/src/doc/display_plugin_tutorial.rst: -------------------------------------------------------------------------------- 1 | ImuDisplay 2 | ========== 3 | 4 | Overview 5 | -------- 6 | 7 | This tutorial shows how to write a simple Display plugin for RViz. 8 | 9 | RViz does not currently have a way to display sensor_msgs/Imu messages 10 | directly. The code in this tutorial implements a subclass of 11 | rviz::Display to do so. 12 | 13 | The source code for this tutorial is in the rviz_plugin_tutorials 14 | package. You can check out the source directly or (if you use Ubuntu) 15 | you can just apt-get install the pre-compiled Debian package like so:: 16 | 17 | sudo apt-get install ros-hydro-visualization-tutorials 18 | 19 | Here is what the new ImuDisplay output looks like, showing a sequence of 20 | sensor_msgs/Imu messages from the test script: 21 | 22 | .. image:: imu_arrows.png 23 | 24 | The Plugin Code 25 | --------------- 26 | 27 | The code for ImuDisplay is in these files: 28 | :srcdir:`src/imu_display.h`, 29 | :srcdir:`src/imu_display.cpp`, 30 | :srcdir:`src/imu_visual.h`, and 31 | :srcdir:`src/imu_visual.cpp`. 32 | 33 | imu_display.h 34 | ^^^^^^^^^^^^^ 35 | 36 | The full text of imu_display.h is here: :srcdir:`src/imu_display.h` 37 | 38 | .. tutorial-formatter:: ../imu_display.h 39 | 40 | imu_display.cpp 41 | ^^^^^^^^^^^^^^^ 42 | 43 | The full text of imu_display.cpp is here: :srcdir:`src/imu_display.cpp` 44 | 45 | .. tutorial-formatter:: ../imu_display.cpp 46 | 47 | imu_visual.h 48 | ^^^^^^^^^^^^ 49 | 50 | The full text of imu_visual.h is here: :srcdir:`src/imu_visual.h` 51 | 52 | .. tutorial-formatter:: ../imu_visual.h 53 | 54 | imu_visual.cpp 55 | ^^^^^^^^^^^^^^ 56 | 57 | The full text of imu_visual.cpp is here: :srcdir:`src/imu_visual.cpp` 58 | 59 | .. tutorial-formatter:: ../imu_visual.cpp 60 | 61 | .. include:: building_and_exporting.rst 62 | 63 | Trying It Out 64 | ------------- 65 | 66 | Once your RViz plugin is compiled and exported, simply run rviz normally:: 67 | 68 | rosrun rviz rviz 69 | 70 | and rviz will use pluginlib to find all the plugins exported to it. 71 | 72 | Add an ImuDisplay by clicking the "Add" button at the bottom of the 73 | "Displays" panel (or by typing Control-N), then scrolling down through 74 | the available displays until you see "Imu" under your plugin package 75 | name (here it is "rviz_plugin_tutorials"). 76 | 77 | .. image:: imu_plugin.png 78 | 79 | If "Imu" is not in your list of Display Types, look through RViz's 80 | console output for error messages relating to plugin loading. Some common 81 | problems are: 82 | 83 | - not having a plugin_description.xml file, 84 | - not exporting it in the package.xml file, or 85 | - not properly referencing the library file (like 86 | librviz_plugin_tutorials.so) from plugin_description.xml. 87 | 88 | Once you've added the Imu display to RViz, you just need to set the 89 | topic name of the display to a source of sensor_msgs/Imu messages. 90 | 91 | If you don't happen to have an IMU or other source of sensor_msgs/Imu 92 | messages, you can test the plugin with a Python script like this: 93 | :srcdir:`scripts/send_test_msgs.py`. 94 | 95 | The script publishes on the "/test_imu" topic, so enter that. 96 | 97 | The script publishes both Imu messages and a moving TF frame 98 | ("/base_link" relative to "/map"), so make sure your "Fixed Frame" is 99 | set to "/map". 100 | 101 | Finally, adjust the "History Length" parameter of the Imu display to 102 | 10 and you should see something like the picture at the top of this 103 | page. 104 | 105 | Note: If you use this to visualize messages from an *actual* IMU, the 106 | arrows are going to be huge compared to most robots: 107 | 108 | .. image:: real_imu.png 109 | 110 | (Note the PR2 robot at the base of the purple arrow.) This is because 111 | the Imu acceleration units are meters per second squared, and gravity 112 | is 9.8 m/s^2, and we haven't applied any scaling or gravity 113 | compensation to the acceleration vectors. 114 | 115 | Next Steps 116 | ---------- 117 | 118 | This ImuDisplay is not yet a terribly useful Display class. Extensions to make it more useful might be: 119 | 120 | - Add a gravity-compensation option to the acceleration vector. 121 | - Visualize more of the data in the Imu messages. 122 | 123 | To add a gravity compensation option, you might take steps like these: 124 | 125 | - Add a new ``rviz::BoolProperty`` to ImuDisplay to store whether the option is on or off. 126 | - Compute the direction of gravity relative to the Imu frame 127 | orientation (as set in ImuVisual::setFrameOrientation()) and 128 | subtract it from the acceleration vector each time in 129 | ImuVisual::setMessage(). 130 | 131 | Since ImuVisual takes complete Imu messages as input, adding 132 | visualizations of more of the Imu data only needs modifications to 133 | ImuVisual. Imu data displays might look like: 134 | 135 | - orientation: An rviz::Axes object at the Imu reference frame, turned to show the orientation. 136 | - angular_velocity: Maybe a line to show the axis of rotation and a 3D arrow curving around it to show the speed of rotation? 137 | - orientation_covariance: Maybe this is an ellipse at the end of each of the X, Y, and Z axes showing the orientation? 138 | - linear_acceleration_covariance: Maybe this is an ellipsoid at the end of the acceleration arrow? 139 | 140 | As all this might be visually cluttered, it may make sense to include 141 | boolean options to enable or disable some of them. 142 | -------------------------------------------------------------------------------- /rviz_plugin_tutorials/src/doc/flags.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-visualization/visualization_tutorials/3141f0dfcb7c34ce57909226d5394ca5fd57a5c7/rviz_plugin_tutorials/src/doc/flags.png -------------------------------------------------------------------------------- /rviz_plugin_tutorials/src/doc/imu_arrows.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-visualization/visualization_tutorials/3141f0dfcb7c34ce57909226d5394ca5fd57a5c7/rviz_plugin_tutorials/src/doc/imu_arrows.png -------------------------------------------------------------------------------- /rviz_plugin_tutorials/src/doc/imu_plugin.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-visualization/visualization_tutorials/3141f0dfcb7c34ce57909226d5394ca5fd57a5c7/rviz_plugin_tutorials/src/doc/imu_plugin.png -------------------------------------------------------------------------------- /rviz_plugin_tutorials/src/doc/index.rst: -------------------------------------------------------------------------------- 1 | RViz Plugin Tutorials 2 | ===================== 3 | 4 | The rviz_plugin_tutorials package builds a plugin library for rviz 5 | containing two main classes: ImuDisplay and TeleopPanel. 6 | 7 | - :doc:`display_plugin_tutorial` is an example of an rviz::Display 8 | subclass allowing rviz to show data from sensor_msgs::Imu messages. 9 | 10 | - :doc:`panel_plugin_tutorial` is an example of an rviz::Panel 11 | subclass which shows a simple control input for sending velocities 12 | to a mobile base. 13 | 14 | - :doc:`tool_plugin_tutorial` is an example of an rviz::Tool 15 | subclass which lets you plant flag markers on the Z=0 plane. 16 | -------------------------------------------------------------------------------- /rviz_plugin_tutorials/src/doc/panel_plugin_tutorial.rst: -------------------------------------------------------------------------------- 1 | TeleopPanel 2 | =========== 3 | 4 | Overview 5 | -------- 6 | 7 | This tutorial shows how to write a simple Panel plugin for RViz. 8 | 9 | A *panel* in RViz is a GUI widget which can be docked in the main 10 | window or floating. It does not show properties in the "Displays" 11 | panel like a *Display*, but it could show things in the 3D scene. 12 | 13 | A panel can be a useful place to put a bunch of application-specific 14 | GUI elements. You could put start and stop buttons for your robot, or 15 | other command or control inputs. 16 | 17 | RViz has a built-in tool to send a goal pose to a path planner, but it 18 | does not have a native way to send velocity commands directly to a 19 | robot base controller. That is what this tutorial shows, a subclass 20 | of rviz::Panel which lets you send velocity commands right to your 21 | robot. 22 | 23 | The source code for this tutorial is in the rviz_plugin_tutorials 24 | package. You can check out the source directly or (if you use Ubuntu) 25 | you can just apt-get install the pre-compiled Debian package like so:: 26 | 27 | sudo apt-get install ros-hydro-visualization-tutorials 28 | 29 | Here is what RViz looks like with the new "Teleop" panel showing on 30 | the left: 31 | 32 | .. image:: teleop_in_rviz.png 33 | 34 | The Plugin Code 35 | --------------- 36 | 37 | The code for TeleopPanel is in these files: 38 | :srcdir:`src/teleop_panel.h`, 39 | :srcdir:`src/teleop_panel.cpp`, 40 | :srcdir:`src/drive_widget.h`, and 41 | :srcdir:`src/drive_widget.cpp`. 42 | 43 | teleop_panel.h 44 | ^^^^^^^^^^^^^^ 45 | 46 | The full text of teleop_panel.h is here: :srcdir:`src/teleop_panel.h` 47 | 48 | .. tutorial-formatter:: ../teleop_panel.h 49 | 50 | teleop_panel.cpp 51 | ^^^^^^^^^^^^^^^^ 52 | 53 | The full text of teleop_panel.cpp is here: :srcdir:`src/teleop_panel.cpp` 54 | 55 | .. tutorial-formatter:: ../teleop_panel.cpp 56 | 57 | drive_widget.h 58 | ^^^^^^^^^^^^^^ 59 | 60 | The full text of drive_widget.h is here: :srcdir:`src/drive_widget.h` 61 | 62 | .. tutorial-formatter:: ../drive_widget.h 63 | 64 | drive_widget.cpp 65 | ^^^^^^^^^^^^^^^^ 66 | 67 | The full text of drive_widget.cpp is here: :srcdir:`src/drive_widget.cpp` 68 | 69 | .. tutorial-formatter:: ../drive_widget.cpp 70 | 71 | .. include:: building_and_exporting.rst 72 | 73 | Trying It Out 74 | ------------- 75 | 76 | Once your RViz plugin is compiled and exported, simply run rviz normally:: 77 | 78 | rosrun rviz rviz 79 | 80 | and rviz will use pluginlib to find all the plugins exported to it. 81 | 82 | Add a Teleop panel by opening the "Panels" menu and then "Add New 83 | Panel" within that. This should bring up a Panel class chooser dialog 84 | with "Teleop" in it (here it is "rviz_plugin_tutorials"): 85 | 86 | .. image:: teleop_plugin.png 87 | 88 | If "Teleop" is not in your list of Panel types, look through RViz's 89 | console output for error messages relating to plugin loading. Some common 90 | problems are: 91 | 92 | - not having a plugin_description.xml file, 93 | - not exporting it in the manifest.xml file, or 94 | - not properly referencing the library file (like 95 | librviz_plugin_tutorials.so) from plugin_description.xml. 96 | 97 | Once you've added the Teleop panel to RViz, you just need to enter a 98 | topic name to publish the geometry_msgs/Twist command velocities on. 99 | Once a non-empty string has been entered in the "Output Topic" field, 100 | the control square area should light up and accept mouse events. 101 | Holding the mouse button down in the control area sends a linear 102 | velocity based on the Y position of the mouse relative to the center 103 | and an angular velocity based on the X position of the mouse relative 104 | to the center. 105 | 106 | Next Steps 107 | ---------- 108 | 109 | This Teleop panel might be useful as it is, since it already sends out 110 | command velocities appropriate for a diff-drive robot. However, there 111 | are a few things which might make it more useful: 112 | 113 | - Adjustable scaling of the linear and angular velocities. 114 | - Enforced maxima for the velocities. 115 | - An adjustable robot width parameter, so that the curved arrows accurately show the arc a robot would traverse. 116 | - A "strafe" mode (maybe when holding down the Shift key) for robots like the PR2 with (more) holonomic drive ability. 117 | -------------------------------------------------------------------------------- /rviz_plugin_tutorials/src/doc/real_imu.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-visualization/visualization_tutorials/3141f0dfcb7c34ce57909226d5394ca5fd57a5c7/rviz_plugin_tutorials/src/doc/real_imu.png -------------------------------------------------------------------------------- /rviz_plugin_tutorials/src/doc/teleop_in_rviz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-visualization/visualization_tutorials/3141f0dfcb7c34ce57909226d5394ca5fd57a5c7/rviz_plugin_tutorials/src/doc/teleop_in_rviz.png -------------------------------------------------------------------------------- /rviz_plugin_tutorials/src/doc/teleop_plugin.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-visualization/visualization_tutorials/3141f0dfcb7c34ce57909226d5394ca5fd57a5c7/rviz_plugin_tutorials/src/doc/teleop_plugin.png -------------------------------------------------------------------------------- /rviz_plugin_tutorials/src/doc/tool_plugin_tutorial.rst: -------------------------------------------------------------------------------- 1 | PlantFlagTool 2 | ============= 3 | 4 | Overview 5 | -------- 6 | 7 | This tutorial shows how to write a new tool for RViz. 8 | 9 | In RViz, a tool is a class that determines how mouse events interact 10 | with the visualizer. In this example we describe PlantFlagTool which 11 | lets you place "flag" markers in the 3D scene. 12 | 13 | The source code for this tutorial is in the rviz_plugin_tutorials 14 | package. You can check out the source directly or (if you use Ubuntu) 15 | you can just apt-get install the pre-compiled Debian package like so:: 16 | 17 | sudo apt-get install ros-hydro-visualization-tutorials 18 | 19 | Here is an example of what the new PlantFlagTool can do: 20 | 21 | .. image:: flags.png 22 | 23 | The Plugin Code 24 | --------------- 25 | 26 | The code for PlantFlagTool is in these files: 27 | :srcdir:`src/plant_flag_tool.h`, and 28 | :srcdir:`src/plant_flag_tool.cpp`. 29 | 30 | plant_flag_tool.h 31 | ^^^^^^^^^^^^^^^^^ 32 | 33 | The full text of plant_flag_tool.h is here: :srcdir:`src/plant_flag_tool.h` 34 | 35 | .. tutorial-formatter:: ../plant_flag_tool.h 36 | 37 | plant_flag_tool.cpp 38 | ^^^^^^^^^^^^^^^^^^^ 39 | 40 | The full text of plant_flag_tool.cpp is here: :srcdir:`src/plant_flag_tool.cpp` 41 | 42 | .. tutorial-formatter:: ../plant_flag_tool.cpp 43 | 44 | .. include:: building_and_exporting.rst 45 | 46 | Trying It Out 47 | ------------- 48 | 49 | Once your RViz plugin is compiled and exported, simply run rviz normally:: 50 | 51 | rosrun rviz rviz 52 | 53 | and rviz will use pluginlib to find all the plugins exported to it. 54 | 55 | Add a PlantFlag tool by clicking on the "+" button in the toolbar and 56 | selecting "PlantFlag" from the list under your plugin package name 57 | (here it is "rviz_plugin_tutorials"). 58 | 59 | Once "PlantFlag" is in your toolbar, click it or press "l" (the 60 | shortcut key) to start planting flags. Open the "Tool Properties" 61 | panel to see the positions of the flags you have planted. 62 | 63 | Currently the only way to remove the flags is to delete the PlantFlag 64 | tool, which you do by pressing the "-" (minus sign) button in the 65 | toolbar and selecting "PlantFlag". 66 | 67 | Next Steps 68 | ---------- 69 | 70 | PlantFlag as shown here is not terribly useful yet. Some extensions to make it more useful might be: 71 | 72 | - Add the ability to delete, re-position, and re-name existing flags. 73 | - Publish ROS messages with the names and locations of the flags. 74 | 75 | To modify existing flags, you might: 76 | 77 | - Change processMouseEvent() to notice when the mouse is pointing near an existing flag. 78 | - When it is: 79 | 80 | - make the flag highlight. 81 | - If the right button is pressed, show a context menu with delete and rename options. 82 | - If the left button is pressed, begin dragging the existing flag around. 83 | 84 | Conclusion 85 | ---------- 86 | 87 | There are many possibilities for new types of interaction with RViz. 88 | We look forward to seeing what you make. 89 | -------------------------------------------------------------------------------- /rviz_plugin_tutorials/src/doc/tutorialformatter.py: -------------------------------------------------------------------------------- 1 | """ 2 | tutorialformatter 3 | =========================== 4 | 5 | This extension provides a directive to include a source code file 6 | in a document, but with certain comments from the file formatted 7 | as regular document text. This allows code for a tutorial to look like: 8 | 9 | /// BEGIN_TUTORIAL 10 | /// This next line adds one. 11 | i = i + 1; 12 | /// Then we need to double it. 13 | i = i * 2; 14 | /// END_TUTORIAL 15 | 16 | And have it formatted as 17 | 18 | This next line adds one.:: 19 | i = i + 1; 20 | 21 | Then we need to double it.:: 22 | i = i * 2; 23 | 24 | The special-looking comment character sequence at the start of 25 | each text line can be anything not starting or ending with 26 | whitespace. tutorialformatter starts by scanning the file for the 27 | string BEGIN_TUTORIAL. When it finds it, it takes all the 28 | characters before BEGIN_TUTORIAL on that line, strips whitespace 29 | from the left, and uses that as the text marker. So this would 30 | also be fine: 31 | 32 | #My Tutorial# BEGIN_TUTORIAL 33 | #My Tutorial# This next line adds one. 34 | i = i + 1 35 | #My Tutorial# Then we need to double it. 36 | i = i * 2 37 | #My Tutorial# END_TUTORIAL 38 | 39 | .. moduleauthor:: Dave Hershberger 40 | """ 41 | 42 | __version__ = '0.1.0' 43 | 44 | import os 45 | from docutils.parsers import rst 46 | from docutils.parsers.rst.directives import flag, unchanged 47 | from docutils.statemachine import string2lines 48 | from pygments.lexers import get_lexer_for_filename 49 | 50 | class TutorialFormatterDirective(rst.Directive): 51 | has_content = False 52 | final_argument_whitespace = True 53 | required_arguments = 1 54 | 55 | option_spec = dict(shell=flag, prompt=flag, nostderr=flag, 56 | in_srcdir=flag, extraargs=unchanged, 57 | until=unchanged) 58 | 59 | def run(self): 60 | filename = self.arguments[0] 61 | text_tag = None 62 | tag_len = 0 63 | 64 | filepath = self.state.document.settings.env.srcdir 65 | absfilename = os.path.join( filepath, filename ) 66 | if absfilename.endswith('.h'): 67 | language = 'c++' 68 | elif absfilename.endswith('CMakeLists.txt'): 69 | language = 'cmake' 70 | else: 71 | try: 72 | language = get_lexer_for_filename( absfilename ).name.lower() 73 | if language == 'text only': 74 | language = 'none' 75 | except: 76 | language = 'none' 77 | code_prefix = '\n.. code-block:: ' + language + '\n\n' 78 | code_suffix = '\n' 79 | 80 | print(f"tutorial-formatter running on {absfilename}") 81 | file_ = open( absfilename, 'r' ) 82 | text_to_process = "" 83 | current_block = "" 84 | in_code = False 85 | in_text = False 86 | in_tutorial = False 87 | for line in file_: 88 | if not in_tutorial: 89 | begin_pos = line.find( 'BEGIN_TUTORIAL' ) 90 | if begin_pos != -1: 91 | text_tag = line[:begin_pos].lstrip() 92 | tag_len = len( text_tag ) 93 | in_tutorial = True 94 | continue 95 | if line.find( 'END_TUTORIAL' ) != -1: 96 | break 97 | stripped = line.lstrip() 98 | if stripped.startswith( text_tag.strip() ): 99 | if in_code: 100 | text_to_process += code_prefix + current_block + code_suffix 101 | current_block = "" 102 | in_code = False 103 | in_text = True 104 | addition = stripped[tag_len:] 105 | if addition == '' or addition[-1] != '\n': 106 | addition += '\n' 107 | current_block += addition 108 | else: 109 | if in_text: 110 | text_to_process += current_block 111 | current_block = "" 112 | in_text = False 113 | in_code = True # Code to show begins right after tagged text 114 | if in_code: 115 | current_block += ' ' + line 116 | if in_code: 117 | text_to_process += code_prefix + current_block + code_suffix 118 | elif in_text: 119 | text_to_process += current_block 120 | 121 | # Debug writes... 122 | #print 'text_to_process =' 123 | #print text_to_process 124 | #print '= text_to_process' 125 | 126 | lines = string2lines( text_to_process ) 127 | self.state_machine.insert_input( lines, absfilename ) 128 | 129 | return [] 130 | 131 | def setup(app): 132 | app.add_directive('tutorial-formatter', TutorialFormatterDirective) 133 | -------------------------------------------------------------------------------- /rviz_plugin_tutorials/src/drive_widget.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2011, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | #include "drive_widget.h" 37 | 38 | namespace rviz_plugin_tutorials 39 | { 40 | 41 | // BEGIN_TUTORIAL 42 | // The DriveWidget constructor does the normal Qt thing of 43 | // passing the parent widget to the superclass constructor, then 44 | // initializing the member variables. 45 | DriveWidget::DriveWidget( QWidget* parent ) 46 | : QWidget( parent ) 47 | , linear_velocity_( 0 ) 48 | , angular_velocity_( 0 ) 49 | , linear_scale_( 10 ) 50 | , angular_scale_( 2 ) 51 | { 52 | } 53 | 54 | // This paintEvent() is complex because of the drawing of the two 55 | // arc-arrows representing wheel motion. It is not particularly 56 | // relevant to learning how to make an RViz plugin, so I will kind of 57 | // skim it. 58 | void DriveWidget::paintEvent( QPaintEvent* event ) 59 | { 60 | // The background color and crosshair lines are drawn differently 61 | // depending on whether this widget is enabled or not. This gives a 62 | // nice visual indication of whether the control is "live". 63 | QColor background; 64 | QColor crosshair; 65 | if( isEnabled() ) 66 | { 67 | background = Qt::white; 68 | crosshair = Qt::black; 69 | } 70 | else 71 | { 72 | background = Qt::lightGray; 73 | crosshair = Qt::darkGray; 74 | } 75 | 76 | // The main visual is a square, centered in the widget's area. Here 77 | // we compute the size of the square and the horizontal and vertical 78 | // offsets of it. 79 | int w = width(); 80 | int h = height(); 81 | int size = (( w > h ) ? h : w) - 1; 82 | int hpad = ( w - size ) / 2; 83 | int vpad = ( h - size ) / 2; 84 | 85 | QPainter painter( this ); 86 | painter.setBrush( background ); 87 | painter.setPen( crosshair ); 88 | 89 | // Draw the background square. 90 | painter.drawRect( QRect( hpad, vpad, size, size )); 91 | 92 | // Draw a cross-hair inside the square. 93 | painter.drawLine( hpad, height() / 2, hpad + size, height() / 2 ); 94 | painter.drawLine( width() / 2, vpad, width() / 2, vpad + size ); 95 | 96 | // If the widget is enabled and the velocities are not zero, draw 97 | // some sweet green arrows showing possible paths that the wheels of 98 | // a diff-drive robot would take if it stayed at these velocities. 99 | if( isEnabled() && (angular_velocity_ != 0 || linear_velocity_ != 0 )) 100 | { 101 | QPen arrow; 102 | arrow.setWidth( size/20 ); 103 | arrow.setColor( Qt::green ); 104 | arrow.setCapStyle( Qt::RoundCap ); 105 | arrow.setJoinStyle( Qt::RoundJoin ); 106 | painter.setPen( arrow ); 107 | 108 | // This code steps along a central arc defined by the linear and 109 | // angular velocites. At each step, it computes where the left 110 | // and right wheels would be and collects the resulting points 111 | // in the left_track and right_track arrays. 112 | const int step_count = 100; 113 | QPointF left_track[ step_count ]; 114 | QPointF right_track[ step_count ]; 115 | 116 | float half_track_width = size/4.0; 117 | 118 | float cx = w/2; 119 | float cy = h/2; 120 | left_track[ 0 ].setX( cx - half_track_width ); 121 | left_track[ 0 ].setY( cy ); 122 | right_track[ 0 ].setX( cx + half_track_width ); 123 | right_track[ 0 ].setY( cy ); 124 | float angle = M_PI/2; 125 | float delta_angle = angular_velocity_ / step_count; 126 | float step_dist = linear_velocity_ * size/2 / linear_scale_ / step_count; 127 | for( int step = 1; step < step_count; step++ ) 128 | { 129 | angle += delta_angle / 2; 130 | float next_cx = cx + step_dist * cosf( angle ); 131 | float next_cy = cy - step_dist * sinf( angle ); 132 | angle += delta_angle / 2; 133 | 134 | left_track[ step ].setX( next_cx + half_track_width * cosf( angle + M_PI/2 )); 135 | left_track[ step ].setY( next_cy - half_track_width * sinf( angle + M_PI/2 )); 136 | right_track[ step ].setX( next_cx + half_track_width * cosf( angle - M_PI/2 )); 137 | right_track[ step ].setY( next_cy - half_track_width * sinf( angle - M_PI/2 )); 138 | 139 | cx = next_cx; 140 | cy = next_cy; 141 | } 142 | // Now the track arrays are filled, so stroke each with a fat green line. 143 | painter.drawPolyline( left_track, step_count ); 144 | painter.drawPolyline( right_track, step_count ); 145 | 146 | // Here we decide which direction each arrowhead will point 147 | // (forward or backward). This works by comparing the arc length 148 | // travelled by the center in one step (step_dist) with the arc 149 | // length travelled by the wheel (half_track_width * delta_angle). 150 | int left_arrow_dir = (-step_dist + half_track_width * delta_angle > 0); 151 | int right_arrow_dir = (-step_dist - half_track_width * delta_angle > 0); 152 | 153 | // Use MiterJoin for the arrowheads so we get a nice sharp point. 154 | arrow.setJoinStyle( Qt::MiterJoin ); 155 | painter.setPen( arrow ); 156 | 157 | // Compute and draw polylines for each arrowhead. This code could 158 | // probably be more elegant. 159 | float head_len = size / 8.0; 160 | QPointF arrow_head[ 3 ]; 161 | float x, y; 162 | if( fabsf( -step_dist + half_track_width * delta_angle ) > .01 ) 163 | { 164 | x = left_track[ step_count - 1 ].x(); 165 | y = left_track[ step_count - 1 ].y(); 166 | arrow_head[ 0 ].setX( x + head_len * cosf( angle + 3*M_PI/4 + left_arrow_dir * M_PI )); 167 | arrow_head[ 0 ].setY( y - head_len * sinf( angle + 3*M_PI/4 + left_arrow_dir * M_PI )); 168 | arrow_head[ 1 ].setX( x ); 169 | arrow_head[ 1 ].setY( y ); 170 | arrow_head[ 2 ].setX( x + head_len * cosf( angle - 3*M_PI/4 + left_arrow_dir * M_PI )); 171 | arrow_head[ 2 ].setY( y - head_len * sinf( angle - 3*M_PI/4 + left_arrow_dir * M_PI )); 172 | painter.drawPolyline( arrow_head, 3 ); 173 | } 174 | if( fabsf( -step_dist - half_track_width * delta_angle ) > .01 ) 175 | { 176 | x = right_track[ step_count - 1 ].x(); 177 | y = right_track[ step_count - 1 ].y(); 178 | arrow_head[ 0 ].setX( x + head_len * cosf( angle + 3*M_PI/4 + right_arrow_dir * M_PI )); 179 | arrow_head[ 0 ].setY( y - head_len * sinf( angle + 3*M_PI/4 + right_arrow_dir * M_PI )); 180 | arrow_head[ 1 ].setX( x ); 181 | arrow_head[ 1 ].setY( y ); 182 | arrow_head[ 2 ].setX( x + head_len * cosf( angle - 3*M_PI/4 + right_arrow_dir * M_PI )); 183 | arrow_head[ 2 ].setY( y - head_len * sinf( angle - 3*M_PI/4 + right_arrow_dir * M_PI )); 184 | painter.drawPolyline( arrow_head, 3 ); 185 | } 186 | } 187 | } 188 | 189 | // Every mouse move event received here sends a velocity because Qt 190 | // only sends us mouse move events if there was previously a 191 | // mouse-press event while in the widget. 192 | void DriveWidget::mouseMoveEvent( QMouseEvent* event ) 193 | { 194 | sendVelocitiesFromMouse( event->x(), event->y(), width(), height() ); 195 | } 196 | 197 | // Mouse-press events should send the velocities too, of course. 198 | void DriveWidget::mousePressEvent( QMouseEvent* event ) 199 | { 200 | sendVelocitiesFromMouse( event->x(), event->y(), width(), height() ); 201 | } 202 | 203 | // When the mouse leaves the widget but the button is still held down, 204 | // we don't get the leaveEvent() because the mouse is "grabbed" (by 205 | // default from Qt). However, when the mouse drags out of the widget 206 | // and then other buttons are pressed (or possibly other 207 | // window-manager things happen), we will get a leaveEvent() but not a 208 | // mouseReleaseEvent(). Without catching this event you can have a 209 | // robot stuck "on" without the user controlling it. 210 | void DriveWidget::leaveEvent( QEvent* event ) 211 | { 212 | stop(); 213 | } 214 | 215 | // The ordinary way to stop: let go of the mouse button. 216 | void DriveWidget::mouseReleaseEvent( QMouseEvent* event ) 217 | { 218 | stop(); 219 | } 220 | 221 | // Compute and emit linear and angular velocities based on Y and X 222 | // mouse positions relative to the central square. 223 | void DriveWidget::sendVelocitiesFromMouse( int x, int y, int width, int height ) 224 | { 225 | int size = (( width > height ) ? height : width ); 226 | int hpad = ( width - size ) / 2; 227 | int vpad = ( height - size ) / 2; 228 | 229 | linear_velocity_ = (1.0 - float( y - vpad ) / float( size / 2 )) * linear_scale_; 230 | angular_velocity_ = (1.0 - float( x - hpad ) / float( size / 2 )) * angular_scale_; 231 | Q_EMIT outputVelocity( linear_velocity_, angular_velocity_ ); 232 | 233 | // update() is a QWidget function which schedules this widget to be 234 | // repainted the next time through the main event loop. We need 235 | // this because the velocities have just changed, so the arrows need 236 | // to be redrawn to match. 237 | update(); 238 | } 239 | 240 | // How to stop: emit velocities of 0! 241 | void DriveWidget::stop() 242 | { 243 | linear_velocity_ = 0; 244 | angular_velocity_ = 0; 245 | Q_EMIT outputVelocity( linear_velocity_, angular_velocity_ ); 246 | update(); 247 | } 248 | // END_TUTORIAL 249 | 250 | } // end namespace rviz_plugin_tutorials 251 | -------------------------------------------------------------------------------- /rviz_plugin_tutorials/src/drive_widget.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2011, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | #ifndef DRIVE_WIDGET_H 30 | #define DRIVE_WIDGET_H 31 | 32 | #include 33 | 34 | namespace rviz_plugin_tutorials 35 | { 36 | 37 | // BEGIN_TUTORIAL 38 | // DriveWidget implements a control which translates mouse Y values 39 | // into linear velocities and mouse X values into angular velocities. 40 | // 41 | // For maximum reusability, this class is only responsible for user 42 | // interaction and display inside its widget. It does not make any 43 | // ROS or RViz calls. It communicates its data to the outside just 44 | // via Qt signals. 45 | class DriveWidget: public QWidget 46 | { 47 | Q_OBJECT 48 | public: 49 | // This class is not instantiated by pluginlib::ClassLoader, so the 50 | // constructor has no restrictions. 51 | DriveWidget( QWidget* parent = 0 ); 52 | 53 | // We override QWidget::paintEvent() to do custom painting. 54 | virtual void paintEvent( QPaintEvent* event ); 55 | 56 | // We override the mouse events and leaveEvent() to keep track of 57 | // what the mouse is doing. 58 | virtual void mouseMoveEvent( QMouseEvent* event ); 59 | virtual void mousePressEvent( QMouseEvent* event ); 60 | virtual void mouseReleaseEvent( QMouseEvent* event ); 61 | virtual void leaveEvent( QEvent* event ); 62 | 63 | // Override sizeHint() to give the layout managers some idea of a 64 | // good size for this. 65 | virtual QSize sizeHint() const { return QSize( 150, 150 ); } 66 | 67 | // We emit outputVelocity() whenever it changes. 68 | Q_SIGNALS: 69 | void outputVelocity( float linear, float angular ); 70 | 71 | // mouseMoveEvent() and mousePressEvent() need the same math to 72 | // figure the velocities, so I put that in here. 73 | protected: 74 | void sendVelocitiesFromMouse( int x, int y, int width, int height ); 75 | 76 | // A function to emit zero velocity. 77 | void stop(); 78 | 79 | // Finally the member variables: 80 | float linear_velocity_; // In m/s 81 | float angular_velocity_; // In radians/s 82 | float linear_scale_; // In m/s 83 | float angular_scale_; // In radians/s 84 | }; 85 | // END_TUTORIAL 86 | 87 | } // end namespace rviz_plugin_tutorials 88 | 89 | 90 | #endif // DRIVE_WIDGET_H 91 | -------------------------------------------------------------------------------- /rviz_plugin_tutorials/src/flag.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | #ifndef FLAG_H 30 | #define FLAG_H 31 | 32 | namespace rviz_plugin_tutorials 33 | { 34 | 35 | class Flag: public rviz::Property 36 | { 37 | Q_OBJECT 38 | public: 39 | Flag(); 40 | }; 41 | 42 | } 43 | 44 | #endif // FLAG_H 45 | -------------------------------------------------------------------------------- /rviz_plugin_tutorials/src/imu_display.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | #include "imu_visual.h" 42 | 43 | #include "imu_display.h" 44 | 45 | namespace rviz_plugin_tutorials 46 | { 47 | 48 | // BEGIN_TUTORIAL 49 | // The constructor must have no arguments, so we can't give the 50 | // constructor the parameters it needs to fully initialize. 51 | ImuDisplay::ImuDisplay() 52 | { 53 | color_property_ = new rviz::ColorProperty( "Color", QColor( 204, 51, 204 ), 54 | "Color to draw the acceleration arrows.", 55 | this, SLOT( updateColorAndAlpha() )); 56 | 57 | alpha_property_ = new rviz::FloatProperty( "Alpha", 1.0, 58 | "0 is fully transparent, 1.0 is fully opaque.", 59 | this, SLOT( updateColorAndAlpha() )); 60 | 61 | history_length_property_ = new rviz::IntProperty( "History Length", 1, 62 | "Number of prior measurements to display.", 63 | this, SLOT( updateHistoryLength() )); 64 | history_length_property_->setMin( 1 ); 65 | history_length_property_->setMax( 100000 ); 66 | } 67 | 68 | // After the top-level rviz::Display::initialize() does its own setup, 69 | // it calls the subclass's onInitialize() function. This is where we 70 | // instantiate all the workings of the class. We make sure to also 71 | // call our immediate super-class's onInitialize() function, since it 72 | // does important stuff setting up the message filter. 73 | // 74 | // Note that "MFDClass" is a typedef of 75 | // ``MessageFilterDisplay``, to save typing that long 76 | // templated class name every time you need to refer to the 77 | // superclass. 78 | void ImuDisplay::onInitialize() 79 | { 80 | MFDClass::onInitialize(); 81 | updateHistoryLength(); 82 | } 83 | 84 | ImuDisplay::~ImuDisplay() 85 | { 86 | } 87 | 88 | // Clear the visuals by deleting their objects. 89 | void ImuDisplay::reset() 90 | { 91 | MFDClass::reset(); 92 | visuals_.clear(); 93 | } 94 | 95 | // Set the current color and alpha values for each visual. 96 | void ImuDisplay::updateColorAndAlpha() 97 | { 98 | float alpha = alpha_property_->getFloat(); 99 | Ogre::ColourValue color = color_property_->getOgreColor(); 100 | 101 | for( size_t i = 0; i < visuals_.size(); i++ ) 102 | { 103 | visuals_[ i ]->setColor( color.r, color.g, color.b, alpha ); 104 | } 105 | } 106 | 107 | // Set the number of past visuals to show. 108 | void ImuDisplay::updateHistoryLength() 109 | { 110 | visuals_.rset_capacity(history_length_property_->getInt()); 111 | } 112 | 113 | // This is our callback to handle an incoming message. 114 | void ImuDisplay::processMessage( const sensor_msgs::Imu::ConstPtr& msg ) 115 | { 116 | // Here we call the rviz::FrameManager to get the transform from the 117 | // fixed frame to the frame in the header of this Imu message. If 118 | // it fails, we can't do anything else so we return. 119 | Ogre::Quaternion orientation; 120 | Ogre::Vector3 position; 121 | if( !context_->getFrameManager()->getTransform( msg->header.frame_id, 122 | msg->header.stamp, 123 | position, orientation )) 124 | { 125 | ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", 126 | msg->header.frame_id.c_str(), qPrintable( fixed_frame_ )); 127 | return; 128 | } 129 | 130 | // We are keeping a circular buffer of visual pointers. This gets 131 | // the next one, or creates and stores it if the buffer is not full 132 | boost::shared_ptr visual; 133 | if( visuals_.full() ) 134 | { 135 | visual = visuals_.front(); 136 | } 137 | else 138 | { 139 | visual.reset(new ImuVisual( context_->getSceneManager(), scene_node_ )); 140 | } 141 | 142 | // Now set or update the contents of the chosen visual. 143 | visual->setMessage( msg ); 144 | visual->setFramePosition( position ); 145 | visual->setFrameOrientation( orientation ); 146 | 147 | float alpha = alpha_property_->getFloat(); 148 | Ogre::ColourValue color = color_property_->getOgreColor(); 149 | visual->setColor( color.r, color.g, color.b, alpha ); 150 | 151 | // And send it to the end of the circular buffer 152 | visuals_.push_back(visual); 153 | } 154 | 155 | } // end namespace rviz_plugin_tutorials 156 | 157 | // Tell pluginlib about this class. It is important to do this in 158 | // global scope, outside our package's namespace. 159 | #include 160 | PLUGINLIB_EXPORT_CLASS(rviz_plugin_tutorials::ImuDisplay,rviz::Display ) 161 | // END_TUTORIAL 162 | -------------------------------------------------------------------------------- /rviz_plugin_tutorials/src/imu_display.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef IMU_DISPLAY_H 31 | #define IMU_DISPLAY_H 32 | 33 | #ifndef Q_MOC_RUN 34 | #include 35 | 36 | #include 37 | #include 38 | #endif 39 | 40 | namespace Ogre 41 | { 42 | class SceneNode; 43 | } 44 | 45 | namespace rviz 46 | { 47 | class ColorProperty; 48 | class FloatProperty; 49 | class IntProperty; 50 | } 51 | 52 | // All the source in this plugin is in its own namespace. This is not 53 | // required but is good practice. 54 | namespace rviz_plugin_tutorials 55 | { 56 | 57 | class ImuVisual; 58 | 59 | // BEGIN_TUTORIAL 60 | // Here we declare our new subclass of rviz::Display. Every display 61 | // which can be listed in the "Displays" panel is a subclass of 62 | // rviz::Display. 63 | // 64 | // ImuDisplay will show a 3D arrow showing the direction and magnitude 65 | // of the IMU acceleration vector. The base of the arrow will be at 66 | // the frame listed in the header of the Imu message, and the 67 | // direction of the arrow will be relative to the orientation of that 68 | // frame. It will also optionally show a history of recent 69 | // acceleration vectors, which will be stored in a circular buffer. 70 | // 71 | // The ImuDisplay class itself just implements the circular buffer, 72 | // editable parameters, and Display subclass machinery. The visuals 73 | // themselves are represented by a separate class, ImuVisual. The 74 | // idiom for the visuals is that when the objects exist, they appear 75 | // in the scene, and when they are deleted, they disappear. 76 | class ImuDisplay: public rviz::MessageFilterDisplay 77 | { 78 | Q_OBJECT 79 | public: 80 | // Constructor. pluginlib::ClassLoader creates instances by calling 81 | // the default constructor, so make sure you have one. 82 | ImuDisplay(); 83 | virtual ~ImuDisplay(); 84 | 85 | // Overrides of protected virtual functions from Display. As much 86 | // as possible, when Displays are not enabled, they should not be 87 | // subscribed to incoming data and should not show anything in the 88 | // 3D view. These functions are where these connections are made 89 | // and broken. 90 | protected: 91 | virtual void onInitialize(); 92 | 93 | // A helper to clear this display back to the initial state. 94 | virtual void reset(); 95 | 96 | // These Qt slots get connected to signals indicating changes in the user-editable properties. 97 | private Q_SLOTS: 98 | void updateColorAndAlpha(); 99 | void updateHistoryLength(); 100 | 101 | // Function to handle an incoming ROS message. 102 | private: 103 | void processMessage( const sensor_msgs::Imu::ConstPtr& msg ); 104 | 105 | // Storage for the list of visuals. It is a circular buffer where 106 | // data gets popped from the front (oldest) and pushed to the back (newest) 107 | boost::circular_buffer > visuals_; 108 | 109 | // User-editable property variables. 110 | rviz::ColorProperty* color_property_; 111 | rviz::FloatProperty* alpha_property_; 112 | rviz::IntProperty* history_length_property_; 113 | }; 114 | // END_TUTORIAL 115 | 116 | } // end namespace rviz_plugin_tutorials 117 | 118 | #endif // IMU_DISPLAY_H 119 | // %EndTag(FULL_SOURCE)% 120 | -------------------------------------------------------------------------------- /rviz_plugin_tutorials/src/imu_visual.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | 36 | #include "imu_visual.h" 37 | 38 | namespace rviz_plugin_tutorials 39 | { 40 | 41 | // BEGIN_TUTORIAL 42 | ImuVisual::ImuVisual( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node ) 43 | { 44 | scene_manager_ = scene_manager; 45 | 46 | // Ogre::SceneNode s form a tree, with each node storing the 47 | // transform (position and orientation) of itself relative to its 48 | // parent. Ogre does the math of combining those transforms when it 49 | // is time to render. 50 | // 51 | // Here we create a node to store the pose of the Imu's header frame 52 | // relative to the RViz fixed frame. 53 | frame_node_ = parent_node->createChildSceneNode(); 54 | 55 | // We create the arrow object within the frame node so that we can 56 | // set its position and direction relative to its header frame. 57 | acceleration_arrow_.reset(new rviz::Arrow( scene_manager_, frame_node_ )); 58 | } 59 | 60 | ImuVisual::~ImuVisual() 61 | { 62 | // Destroy the frame node since we don't need it anymore. 63 | scene_manager_->destroySceneNode( frame_node_ ); 64 | } 65 | 66 | void ImuVisual::setMessage( const sensor_msgs::Imu::ConstPtr& msg ) 67 | { 68 | const geometry_msgs::Vector3& a = msg->linear_acceleration; 69 | 70 | // Convert the geometry_msgs::Vector3 to an Ogre::Vector3. 71 | Ogre::Vector3 acc( a.x, a.y, a.z ); 72 | 73 | // Find the magnitude of the acceleration vector. 74 | float length = acc.length(); 75 | 76 | // Scale the arrow's thickness in each dimension along with its length. 77 | Ogre::Vector3 scale( length, length, length ); 78 | acceleration_arrow_->setScale( scale ); 79 | 80 | // Set the orientation of the arrow to match the direction of the 81 | // acceleration vector. 82 | acceleration_arrow_->setDirection( acc ); 83 | } 84 | 85 | // Position and orientation are passed through to the SceneNode. 86 | void ImuVisual::setFramePosition( const Ogre::Vector3& position ) 87 | { 88 | frame_node_->setPosition( position ); 89 | } 90 | 91 | void ImuVisual::setFrameOrientation( const Ogre::Quaternion& orientation ) 92 | { 93 | frame_node_->setOrientation( orientation ); 94 | } 95 | 96 | // Color is passed through to the Arrow object. 97 | void ImuVisual::setColor( float r, float g, float b, float a ) 98 | { 99 | acceleration_arrow_->setColor( r, g, b, a ); 100 | } 101 | // END_TUTORIAL 102 | 103 | } // end namespace rviz_plugin_tutorials 104 | 105 | -------------------------------------------------------------------------------- /rviz_plugin_tutorials/src/imu_visual.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef IMU_VISUAL_H 31 | #define IMU_VISUAL_H 32 | 33 | #include 34 | #include 35 | 36 | namespace rviz 37 | { 38 | class Arrow; 39 | } 40 | 41 | namespace rviz_plugin_tutorials 42 | { 43 | 44 | // BEGIN_TUTORIAL 45 | // Declare the visual class for this display. 46 | // 47 | // Each instance of ImuVisual represents the visualization of a single 48 | // sensor_msgs::Imu message. Currently it just shows an arrow with 49 | // the direction and magnitude of the acceleration vector, but could 50 | // easily be expanded to include more of the message data. 51 | class ImuVisual 52 | { 53 | public: 54 | // Constructor. Creates the visual stuff and puts it into the 55 | // scene, but in an unconfigured state. 56 | ImuVisual( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node ); 57 | 58 | // Destructor. Removes the visual stuff from the scene. 59 | virtual ~ImuVisual(); 60 | 61 | // Configure the visual to show the data in the message. 62 | void setMessage( const sensor_msgs::Imu::ConstPtr& msg ); 63 | 64 | // Set the pose of the coordinate frame the message refers to. 65 | // These could be done inside setMessage(), but that would require 66 | // calls to FrameManager and error handling inside setMessage(), 67 | // which doesn't seem as clean. This way ImuVisual is only 68 | // responsible for visualization. 69 | void setFramePosition( const Ogre::Vector3& position ); 70 | void setFrameOrientation( const Ogre::Quaternion& orientation ); 71 | 72 | // Set the color and alpha of the visual, which are user-editable 73 | // parameters and therefore don't come from the Imu message. 74 | void setColor( float r, float g, float b, float a ); 75 | 76 | private: 77 | // The object implementing the actual arrow shape 78 | boost::shared_ptr acceleration_arrow_; 79 | 80 | // A SceneNode whose pose is set to match the coordinate frame of 81 | // the Imu message header. 82 | Ogre::SceneNode* frame_node_; 83 | 84 | // The SceneManager, kept here only so the destructor can ask it to 85 | // destroy the ``frame_node_``. 86 | Ogre::SceneManager* scene_manager_; 87 | }; 88 | // END_TUTORIAL 89 | 90 | } // end namespace rviz_plugin_tutorials 91 | 92 | #endif // IMU_VISUAL_H 93 | -------------------------------------------------------------------------------- /rviz_plugin_tutorials/src/plant_flag_tool.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2011, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include "plant_flag_tool.h" 43 | 44 | namespace rviz_plugin_tutorials 45 | { 46 | 47 | // BEGIN_TUTORIAL 48 | // Construction and destruction 49 | // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 50 | // 51 | // The constructor must have no arguments, so we can't give the 52 | // constructor the parameters it needs to fully initialize. 53 | // 54 | // Here we set the "shortcut_key_" member variable defined in the 55 | // superclass to declare which key will activate the tool. 56 | PlantFlagTool::PlantFlagTool() 57 | : moving_flag_node_( NULL ) 58 | , current_flag_property_( NULL ) 59 | { 60 | shortcut_key_ = 'l'; 61 | } 62 | 63 | // The destructor destroys the Ogre scene nodes for the flags so they 64 | // disappear from the 3D scene. The destructor for a Tool subclass is 65 | // only called when the tool is removed from the toolbar with the "-" 66 | // button. 67 | PlantFlagTool::~PlantFlagTool() 68 | { 69 | for( unsigned i = 0; i < flag_nodes_.size(); i++ ) 70 | { 71 | scene_manager_->destroySceneNode( flag_nodes_[ i ]); 72 | } 73 | } 74 | 75 | // onInitialize() is called by the superclass after scene_manager_ and 76 | // context_ are set. It should be called only once per instantiation. 77 | // This is where most one-time initialization work should be done. 78 | // onInitialize() is called during initial instantiation of the tool 79 | // object. At this point the tool has not been activated yet, so any 80 | // scene objects created should be invisible or disconnected from the 81 | // scene at this point. 82 | // 83 | // In this case we load a mesh object with the shape and appearance of 84 | // the flag, create an Ogre::SceneNode for the moving flag, and then 85 | // set it invisible. 86 | void PlantFlagTool::onInitialize() 87 | { 88 | flag_resource_ = "package://rviz_plugin_tutorials/media/flag.dae"; 89 | 90 | if( rviz::loadMeshFromResource( flag_resource_ ).isNull() ) 91 | { 92 | ROS_ERROR( "PlantFlagTool: failed to load model resource '%s'.", flag_resource_.c_str() ); 93 | return; 94 | } 95 | 96 | moving_flag_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode(); 97 | Ogre::Entity* entity = scene_manager_->createEntity( flag_resource_ ); 98 | moving_flag_node_->attachObject( entity ); 99 | moving_flag_node_->setVisible( false ); 100 | } 101 | 102 | // Activation and deactivation 103 | // ^^^^^^^^^^^^^^^^^^^^^^^^^^^ 104 | // 105 | // activate() is called when the tool is started by the user, either 106 | // by clicking on its button in the toolbar or by pressing its hotkey. 107 | // 108 | // First we set the moving flag node to be visible, then we create an 109 | // rviz::VectorProperty to show the user the position of the flag. 110 | // Unlike rviz::Display, rviz::Tool is not a subclass of 111 | // rviz::Property, so when we want to add a tool property we need to 112 | // get the parent container with getPropertyContainer() and add it to 113 | // that. 114 | // 115 | // We wouldn't have to set current_flag_property_ to be read-only, but 116 | // if it were writable the flag should really change position when the 117 | // user edits the property. This is a fine idea, and is possible, but 118 | // is left as an exercise for the reader. 119 | void PlantFlagTool::activate() 120 | { 121 | if( moving_flag_node_ ) 122 | { 123 | moving_flag_node_->setVisible( true ); 124 | 125 | current_flag_property_ = new rviz::VectorProperty( "Flag " + QString::number( flag_nodes_.size() )); 126 | current_flag_property_->setReadOnly( true ); 127 | getPropertyContainer()->addChild( current_flag_property_ ); 128 | } 129 | } 130 | 131 | // deactivate() is called when the tool is being turned off because 132 | // another tool has been chosen. 133 | // 134 | // We make the moving flag invisible, then delete the current flag 135 | // property. Deleting a property also removes it from its parent 136 | // property, so that doesn't need to be done in a separate step. If 137 | // we didn't delete it here, it would stay in the list of flags when 138 | // we switch to another tool. 139 | void PlantFlagTool::deactivate() 140 | { 141 | if( moving_flag_node_ ) 142 | { 143 | moving_flag_node_->setVisible( false ); 144 | delete current_flag_property_; 145 | current_flag_property_ = NULL; 146 | } 147 | } 148 | 149 | // Handling mouse events 150 | // ^^^^^^^^^^^^^^^^^^^^^ 151 | // 152 | // processMouseEvent() is sort of the main function of a Tool, because 153 | // mouse interactions are the point of Tools. 154 | // 155 | // We use the utility function rviz::getPointOnPlaneFromWindowXY() to 156 | // see where on the ground plane the user's mouse is pointing, then 157 | // move the moving flag to that point and update the VectorProperty. 158 | // 159 | // If this mouse event was a left button press, we want to save the 160 | // current flag location. Therefore we make a new flag at the same 161 | // place and drop the pointer to the VectorProperty. Dropping the 162 | // pointer means when the tool is deactivated the VectorProperty won't 163 | // be deleted, which is what we want. 164 | int PlantFlagTool::processMouseEvent( rviz::ViewportMouseEvent& event ) 165 | { 166 | if( !moving_flag_node_ ) 167 | { 168 | return Render; 169 | } 170 | Ogre::Vector3 intersection; 171 | Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0.0f ); 172 | if( rviz::getPointOnPlaneFromWindowXY( event.viewport, 173 | ground_plane, 174 | event.x, event.y, intersection )) 175 | { 176 | moving_flag_node_->setVisible( true ); 177 | moving_flag_node_->setPosition( intersection ); 178 | current_flag_property_->setVector( intersection ); 179 | 180 | if( event.leftDown() ) 181 | { 182 | makeFlag( intersection ); 183 | current_flag_property_ = NULL; // Drop the reference so that deactivate() won't remove it. 184 | return Render | Finished; 185 | } 186 | } 187 | else 188 | { 189 | moving_flag_node_->setVisible( false ); // If the mouse is not pointing at the ground plane, don't show the flag. 190 | } 191 | return Render; 192 | } 193 | 194 | // This is a helper function to create a new flag in the Ogre scene and save its scene node in a list. 195 | void PlantFlagTool::makeFlag( const Ogre::Vector3& position ) 196 | { 197 | if( rviz::loadMeshFromResource( flag_resource_ ).isNull() ) 198 | { 199 | ROS_ERROR( "PlantFlagTool: failed to load model resource '%s'.", flag_resource_.c_str() ); 200 | return; 201 | } 202 | 203 | Ogre::SceneNode* node = scene_manager_->getRootSceneNode()->createChildSceneNode(); 204 | Ogre::Entity* entity = scene_manager_->createEntity( flag_resource_ ); 205 | node->attachObject( entity ); 206 | node->setVisible( true ); 207 | node->setPosition( position ); 208 | flag_nodes_.push_back( node ); 209 | } 210 | 211 | // Loading and saving the flags 212 | // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 213 | // 214 | // Tools with a fixed set of Property objects representing adjustable 215 | // parameters are typically just created in the tool's constructor and 216 | // added to the Property container (getPropertyContainer()). In that 217 | // case, the Tool subclass does not need to override load() and save() 218 | // because the default behavior is to read all the Properties in the 219 | // container from the Config object. 220 | // 221 | // Here however, we have a list of named flag positions of unknown 222 | // length, so we need to implement save() and load() ourselves. 223 | // 224 | // We first save the class ID to the config object so the 225 | // rviz::ToolManager will know what to instantiate when the config 226 | // file is read back in. 227 | void PlantFlagTool::save( rviz::Config config ) const 228 | { 229 | config.mapSetValue( "Class", getClassId() ); 230 | 231 | // The top level of this tool's Config is a map, but our flags 232 | // should go in a list, since they may or may not have unique keys. 233 | // Therefore we make a child of the map (``flags_config``) to store 234 | // the list. 235 | rviz::Config flags_config = config.mapMakeChild( "Flags" ); 236 | 237 | // To read the positions and names of the flags, we loop over the 238 | // the children of our Property container: 239 | rviz::Property* container = getPropertyContainer(); 240 | int num_children = container->numChildren(); 241 | for( int i = 0; i < num_children; i++ ) 242 | { 243 | rviz::Property* position_prop = container->childAt( i ); 244 | // For each Property, we create a new Config object representing a 245 | // single flag and append it to the Config list. 246 | rviz::Config flag_config = flags_config.listAppendNew(); 247 | // Into the flag's config we store its name: 248 | flag_config.mapSetValue( "Name", position_prop->getName() ); 249 | // ... and its position. 250 | position_prop->save( flag_config ); 251 | } 252 | } 253 | 254 | // In a tool's load() function, we don't need to read its class 255 | // because that has already been read and used to instantiate the 256 | // object before this can have been called. 257 | void PlantFlagTool::load( const rviz::Config& config ) 258 | { 259 | // Here we get the "Flags" sub-config from the tool config and loop over its entries: 260 | rviz::Config flags_config = config.mapGetChild( "Flags" ); 261 | int num_flags = flags_config.listLength(); 262 | for( int i = 0; i < num_flags; i++ ) 263 | { 264 | rviz::Config flag_config = flags_config.listChildAt( i ); 265 | // At this point each ``flag_config`` represents a single flag. 266 | // 267 | // Here we provide a default name in case the name is not in the config file for some reason: 268 | QString name = "Flag " + QString::number( i + 1 ); 269 | // Then we use the convenience function mapGetString() to read the 270 | // name from ``flag_config`` if it is there. (If no "Name" entry 271 | // were present it would return false, but we don't care about 272 | // that because we have already set a default.) 273 | flag_config.mapGetString( "Name", &name ); 274 | // Given the name we can create an rviz::VectorProperty to display the position: 275 | rviz::VectorProperty* prop = new rviz::VectorProperty( name ); 276 | // Then we just tell the property to read its contents from the config, and we've read all the data. 277 | prop->load( flag_config ); 278 | // We finish each flag by marking it read-only (as discussed 279 | // above), adding it to the property container, and finally making 280 | // an actual visible flag object in the 3D scene at the correct 281 | // position. 282 | prop->setReadOnly( true ); 283 | getPropertyContainer()->addChild( prop ); 284 | makeFlag( prop->getVector() ); 285 | } 286 | } 287 | 288 | // End of .cpp file 289 | // ^^^^^^^^^^^^^^^^ 290 | // 291 | // At the end of every plugin class implementation, we end the 292 | // namespace and then tell pluginlib about the class. It is important 293 | // to do this in global scope, outside our package's namespace. 294 | 295 | } // end namespace rviz_plugin_tutorials 296 | 297 | #include 298 | PLUGINLIB_EXPORT_CLASS(rviz_plugin_tutorials::PlantFlagTool,rviz::Tool ) 299 | // END_TUTORIAL 300 | -------------------------------------------------------------------------------- /rviz_plugin_tutorials/src/plant_flag_tool.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | #ifndef PLANT_FLAG_TOOL_H 30 | #define PLANT_FLAG_TOOL_H 31 | 32 | #include 33 | #include 34 | 35 | namespace rviz 36 | { 37 | class VectorProperty; 38 | class VisualizationManager; 39 | class ViewportMouseEvent; 40 | } 41 | 42 | namespace rviz_plugin_tutorials 43 | { 44 | 45 | // BEGIN_TUTORIAL 46 | // Here we declare our new subclass of rviz::Tool. Every tool 47 | // which can be added to the tool bar is a subclass of 48 | // rviz::Tool. 49 | class PlantFlagTool: public rviz::Tool 50 | { 51 | Q_OBJECT 52 | public: 53 | PlantFlagTool(); 54 | ~PlantFlagTool(); 55 | 56 | virtual void onInitialize(); 57 | 58 | virtual void activate(); 59 | virtual void deactivate(); 60 | 61 | virtual int processMouseEvent( rviz::ViewportMouseEvent& event ); 62 | 63 | virtual void load( const rviz::Config& config ); 64 | virtual void save( rviz::Config config ) const; 65 | 66 | private: 67 | void makeFlag( const Ogre::Vector3& position ); 68 | 69 | std::vector flag_nodes_; 70 | Ogre::SceneNode* moving_flag_node_; 71 | std::string flag_resource_; 72 | rviz::VectorProperty* current_flag_property_; 73 | }; 74 | // END_TUTORIAL 75 | 76 | } // end namespace rviz_plugin_tutorials 77 | 78 | #endif // PLANT_FLAG_TOOL_H 79 | -------------------------------------------------------------------------------- /rviz_plugin_tutorials/src/teleop_panel.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2011, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | 41 | #include "drive_widget.h" 42 | #include "teleop_panel.h" 43 | 44 | namespace rviz_plugin_tutorials 45 | { 46 | 47 | // BEGIN_TUTORIAL 48 | // Here is the implementation of the TeleopPanel class. TeleopPanel 49 | // has these responsibilities: 50 | // 51 | // - Act as a container for GUI elements DriveWidget and QLineEdit. 52 | // - Publish command velocities 10 times per second (whether 0 or not). 53 | // - Saving and restoring internal state from a config file. 54 | // 55 | // We start with the constructor, doing the standard Qt thing of 56 | // passing the optional *parent* argument on to the superclass 57 | // constructor, and also zero-ing the velocities we will be 58 | // publishing. 59 | TeleopPanel::TeleopPanel( QWidget* parent ) 60 | : rviz::Panel( parent ) 61 | , linear_velocity_( 0 ) 62 | , angular_velocity_( 0 ) 63 | { 64 | // Next we lay out the "output topic" text entry field using a 65 | // QLabel and a QLineEdit in a QHBoxLayout. 66 | QHBoxLayout* topic_layout = new QHBoxLayout; 67 | topic_layout->addWidget( new QLabel( "Output Topic:" )); 68 | output_topic_editor_ = new QLineEdit; 69 | topic_layout->addWidget( output_topic_editor_ ); 70 | 71 | // Then create the control widget. 72 | drive_widget_ = new DriveWidget; 73 | 74 | // Lay out the topic field above the control widget. 75 | QVBoxLayout* layout = new QVBoxLayout; 76 | layout->addLayout( topic_layout ); 77 | layout->addWidget( drive_widget_ ); 78 | setLayout( layout ); 79 | 80 | // Create a timer for sending the output. Motor controllers want to 81 | // be reassured frequently that they are doing the right thing, so 82 | // we keep re-sending velocities even when they aren't changing. 83 | // 84 | // Here we take advantage of QObject's memory management behavior: 85 | // since "this" is passed to the new QTimer as its parent, the 86 | // QTimer is deleted by the QObject destructor when this TeleopPanel 87 | // object is destroyed. Therefore we don't need to keep a pointer 88 | // to the timer. 89 | QTimer* output_timer = new QTimer( this ); 90 | 91 | // Next we make signal/slot connections. 92 | connect( drive_widget_, SIGNAL( outputVelocity( float, float )), this, SLOT( setVel( float, float ))); 93 | connect( output_topic_editor_, SIGNAL( editingFinished() ), this, SLOT( updateTopic() )); 94 | connect( output_timer, SIGNAL( timeout() ), this, SLOT( sendVel() )); 95 | 96 | // Start the timer. 97 | output_timer->start( 100 ); 98 | 99 | // Make the control widget start disabled, since we don't start with an output topic. 100 | drive_widget_->setEnabled( false ); 101 | } 102 | 103 | // setVel() is connected to the DriveWidget's output, which is sent 104 | // whenever it changes due to a mouse event. This just records the 105 | // values it is given. The data doesn't actually get sent until the 106 | // next timer callback. 107 | void TeleopPanel::setVel( float lin, float ang ) 108 | { 109 | linear_velocity_ = lin; 110 | angular_velocity_ = ang; 111 | } 112 | 113 | // Read the topic name from the QLineEdit and call setTopic() with the 114 | // results. This is connected to QLineEdit::editingFinished() which 115 | // fires when the user presses Enter or Tab or otherwise moves focus 116 | // away. 117 | void TeleopPanel::updateTopic() 118 | { 119 | setTopic( output_topic_editor_->text() ); 120 | } 121 | 122 | // Set the topic name we are publishing to. 123 | void TeleopPanel::setTopic( const QString& new_topic ) 124 | { 125 | // Only take action if the name has changed. 126 | if( new_topic != output_topic_ ) 127 | { 128 | output_topic_ = new_topic; 129 | // If the topic is the empty string, don't publish anything. 130 | if( output_topic_ == "" ) 131 | { 132 | velocity_publisher_.shutdown(); 133 | } 134 | else 135 | { 136 | // The old ``velocity_publisher_`` is destroyed by this assignment, 137 | // and thus the old topic advertisement is removed. The call to 138 | // nh_advertise() says we want to publish data on the new topic 139 | // name. 140 | velocity_publisher_ = nh_.advertise( output_topic_.toStdString(), 1 ); 141 | } 142 | // rviz::Panel defines the configChanged() signal. Emitting it 143 | // tells RViz that something in this panel has changed that will 144 | // affect a saved config file. Ultimately this signal can cause 145 | // QWidget::setWindowModified(true) to be called on the top-level 146 | // rviz::VisualizationFrame, which causes a little asterisk ("*") 147 | // to show in the window's title bar indicating unsaved changes. 148 | Q_EMIT configChanged(); 149 | } 150 | 151 | // Gray out the control widget when the output topic is empty. 152 | drive_widget_->setEnabled( output_topic_ != "" ); 153 | } 154 | 155 | // Publish the control velocities if ROS is not shutting down and the 156 | // publisher is ready with a valid topic name. 157 | void TeleopPanel::sendVel() 158 | { 159 | if( ros::ok() && velocity_publisher_ ) 160 | { 161 | geometry_msgs::Twist msg; 162 | msg.linear.x = linear_velocity_; 163 | msg.linear.y = 0; 164 | msg.linear.z = 0; 165 | msg.angular.x = 0; 166 | msg.angular.y = 0; 167 | msg.angular.z = angular_velocity_; 168 | velocity_publisher_.publish( msg ); 169 | } 170 | } 171 | 172 | // Save all configuration data from this panel to the given 173 | // Config object. It is important here that you call save() 174 | // on the parent class so the class id and panel name get saved. 175 | void TeleopPanel::save( rviz::Config config ) const 176 | { 177 | rviz::Panel::save( config ); 178 | config.mapSetValue( "Topic", output_topic_ ); 179 | } 180 | 181 | // Load all configuration data for this panel from the given Config object. 182 | void TeleopPanel::load( const rviz::Config& config ) 183 | { 184 | rviz::Panel::load( config ); 185 | QString topic; 186 | if( config.mapGetString( "Topic", &topic )) 187 | { 188 | output_topic_editor_->setText( topic ); 189 | updateTopic(); 190 | } 191 | } 192 | 193 | } // end namespace rviz_plugin_tutorials 194 | 195 | // Tell pluginlib about this class. Every class which should be 196 | // loadable by pluginlib::ClassLoader must have these two lines 197 | // compiled in its .cpp file, outside of any namespace scope. 198 | #include 199 | PLUGINLIB_EXPORT_CLASS(rviz_plugin_tutorials::TeleopPanel,rviz::Panel ) 200 | // END_TUTORIAL 201 | -------------------------------------------------------------------------------- /rviz_plugin_tutorials/src/teleop_panel.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2011, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | #ifndef TELEOP_PANEL_H 30 | #define TELEOP_PANEL_H 31 | 32 | #ifndef Q_MOC_RUN 33 | # include 34 | 35 | # include 36 | #endif 37 | 38 | class QLineEdit; 39 | 40 | namespace rviz_plugin_tutorials 41 | { 42 | 43 | class DriveWidget; 44 | 45 | // BEGIN_TUTORIAL 46 | // Here we declare our new subclass of rviz::Panel. Every panel which 47 | // can be added via the Panels/Add_New_Panel menu is a subclass of 48 | // rviz::Panel. 49 | // 50 | // TeleopPanel will show a text-entry field to set the output topic 51 | // and a 2D control area. The 2D control area is implemented by the 52 | // DriveWidget class, and is described there. 53 | class TeleopPanel: public rviz::Panel 54 | { 55 | // This class uses Qt slots and is a subclass of QObject, so it needs 56 | // the Q_OBJECT macro. 57 | Q_OBJECT 58 | public: 59 | // QWidget subclass constructors usually take a parent widget 60 | // parameter (which usually defaults to 0). At the same time, 61 | // pluginlib::ClassLoader creates instances by calling the default 62 | // constructor (with no arguments). Taking the parameter and giving 63 | // a default of 0 lets the default constructor work and also lets 64 | // someone using the class for something else to pass in a parent 65 | // widget as they normally would with Qt. 66 | TeleopPanel( QWidget* parent = 0 ); 67 | 68 | // Now we declare overrides of rviz::Panel functions for saving and 69 | // loading data from the config file. Here the data is the 70 | // topic name. 71 | virtual void load( const rviz::Config& config ); 72 | virtual void save( rviz::Config config ) const; 73 | 74 | // Next come a couple of public Qt slots. 75 | public Q_SLOTS: 76 | // The control area, DriveWidget, sends its output to a Qt signal 77 | // for ease of re-use, so here we declare a Qt slot to receive it. 78 | void setVel( float linear_velocity_, float angular_velocity_ ); 79 | 80 | // In this example setTopic() does not get connected to any signal 81 | // (it is called directly), but it is easy to define it as a public 82 | // slot instead of a private function in case it would be useful to 83 | // some other user. 84 | void setTopic( const QString& topic ); 85 | 86 | // Here we declare some internal slots. 87 | protected Q_SLOTS: 88 | // sendvel() publishes the current velocity values to a ROS 89 | // topic. Internally this is connected to a timer which calls it 10 90 | // times per second. 91 | void sendVel(); 92 | 93 | // updateTopic() reads the topic name from the QLineEdit and calls 94 | // setTopic() with the result. 95 | void updateTopic(); 96 | 97 | // Then we finish up with protected member variables. 98 | protected: 99 | // The control-area widget which turns mouse events into command 100 | // velocities. 101 | DriveWidget* drive_widget_; 102 | 103 | // One-line text editor for entering the outgoing ROS topic name. 104 | QLineEdit* output_topic_editor_; 105 | 106 | // The current name of the output topic. 107 | QString output_topic_; 108 | 109 | // The ROS publisher for the command velocity. 110 | ros::Publisher velocity_publisher_; 111 | 112 | // The ROS node handle. 113 | ros::NodeHandle nh_; 114 | 115 | // The latest velocity values from the drive widget. 116 | float linear_velocity_; 117 | float angular_velocity_; 118 | // END_TUTORIAL 119 | }; 120 | 121 | } // end namespace rviz_plugin_tutorials 122 | 123 | #endif // TELEOP_PANEL_H 124 | -------------------------------------------------------------------------------- /rviz_python_tutorial/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rviz_python_tutorial 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.11.2 (2025-04-26) 6 | ------------------- 7 | * Eliminate Python 2 code (`#88 `_) 8 | * Contributors: vineet131 9 | 10 | 0.11.1 (2025-04-10) 11 | ------------------- 12 | * Update maintainers (`#63 `_) 13 | * Contributors: Mabel Zhang 14 | 15 | 0.11.0 (2020-05-13) 16 | ------------------- 17 | 18 | 0.10.4 (2020-05-13) 19 | ------------------- 20 | * Updated RViz import (`#60 `_) 21 | * Updated to use ``catkin_install_python()`` (`#59 `_) 22 | * Updated required CMake version to avoid CMP0048 warning (`#57 `_) 23 | * Contributors: Alejandro Hernández Cordero, Shane Loretz 24 | 25 | 0.10.3 (2018-05-09) 26 | ------------------- 27 | * Fixed QWidget not defined bug in rviz_python_tutorial (`#41 `_) 28 | * Contributors: Zihan Chen 29 | 30 | 0.10.2 (2018-01-05) 31 | ------------------- 32 | 33 | 0.10.1 (2016-04-21) 34 | ------------------- 35 | 36 | 0.10.0 (2016-04-21) 37 | ------------------- 38 | 39 | 0.9.2 (2015-09-21) 40 | ------------------ 41 | 42 | 0.9.1 (2015-01-26) 43 | ------------------ 44 | 45 | 0.9.0 (2014-03-24) 46 | ------------------ 47 | * set myself (william) as maintainer 48 | * Contributors: William Woodall 49 | -------------------------------------------------------------------------------- /rviz_python_tutorial/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(rviz_python_tutorial) 3 | find_package(catkin REQUIRED COMPONENTS rviz) 4 | catkin_package() 5 | 6 | ## Install rules 7 | 8 | install(FILES 9 | config.myviz 10 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 11 | 12 | catkin_install_python(PROGRAMS myviz.py 13 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 14 | -------------------------------------------------------------------------------- /rviz_python_tutorial/config.myviz: -------------------------------------------------------------------------------- 1 | Title: RViz Python Tutorial 2 | Panels: 3 | [] 4 | Visualization Manager: 5 | Class: "" 6 | Displays: 7 | - Alpha: 0.5 8 | Cell Size: 1 9 | Class: rviz/Grid 10 | Color: 160; 160; 164 11 | Enabled: true 12 | Line Style: 13 | Line Width: 0.03 14 | Value: Billboards 15 | Name: Grid 16 | Normal Cell Count: 0 17 | Offset: 18 | Value: 0; 0; 0 19 | X: 0 20 | Y: 0 21 | Z: 0 22 | Plane: XY 23 | Plane Cell Count: 10 24 | Reference Frame: 25 | Value: true 26 | Enabled: true 27 | Global Options: 28 | Background Color: 48; 48; 48 29 | Fixed Frame: /map 30 | Name: root 31 | Tools: 32 | - Class: rviz/MoveCamera 33 | - Class: rviz/Interact 34 | - Class: rviz/Select 35 | - Class: rviz/SetInitialPose 36 | Topic: /initialpose 37 | - Class: rviz/SetGoal 38 | Topic: /move_base_simple/goal 39 | Value: true 40 | Views: 41 | Current: 42 | Class: rviz/Orbit 43 | Distance: 23.7169 44 | Focal Point: 45 | Value: 2.265e-06; 3.8147e-06; -3.8147e-06 46 | X: 2.26498e-06 47 | Y: 3.8147e-06 48 | Z: -3.8147e-06 49 | Name: Current View 50 | Near Clip Distance: 0.01 51 | Pitch: 0.349797 52 | Target Frame: 53 | Value: Orbit (rviz) 54 | Yaw: 2.6004 55 | Saved: 56 | - Class: rviz/Orbit 57 | Distance: 23.7169 58 | Focal Point: 59 | Value: 2.265e-06; 3.8147e-06; -3.8147e-06 60 | X: 2.26498e-06 61 | Y: 3.8147e-06 62 | Z: -3.8147e-06 63 | Name: Top View 64 | Near Clip Distance: 0.01 65 | Pitch: 1.5698 66 | Target Frame: 67 | Value: Orbit (rviz) 68 | Yaw: 1.5654 69 | - Class: rviz/Orbit 70 | Distance: 23.7169 71 | Focal Point: 72 | Value: 2.265e-06; 3.8147e-06; -3.8147e-06 73 | X: 2.26498e-06 74 | Y: 3.8147e-06 75 | Z: -3.8147e-06 76 | Name: Side View 77 | Near Clip Distance: 0.01 78 | Pitch: 0.0347963 79 | Target Frame: 80 | Value: Orbit (rviz) 81 | Yaw: 1.5854 82 | Window Geometry: 83 | Height: 1406 84 | Hide Left Dock: false 85 | Hide Right Dock: false 86 | QMainWindow State: 000000ff00000000fd00000004000000000000013c00000487fc0200000007fb000000100044006900730070006c0061007900730100000041000002930000000000000000fb0000001200530065006c0065006300740069006f006e01000000410000013a0000000000000000fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000c00540065006c0065006f00700100000041000004870000000000000000fb0000000c0042007500740074006f006e01000003de000001320000000000000000000000010000010f00000487fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000041000004870000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000002000004b0000000ddfc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ce0000006afc0100000003fb0000000800540069006d00650000000000000004b00000000000000000fb0000000800540069006d00650100000000000004500000000000000000fb0000001c0045007800740072006100200043006f006e00740072006f006c00730000000000000005ce000000c200ffffff000005ce0000051f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 87 | Width: 1508 88 | X: 134 89 | Y: 52 90 | -------------------------------------------------------------------------------- /rviz_python_tutorial/doc-src/conf.py: -------------------------------------------------------------------------------- 1 | import sys, os 2 | 3 | sys.path += [ os.path.abspath( '.' )] 4 | 5 | extensions = [ 'sphinx.ext.extlinks', 6 | 'tutorialformatter' ] 7 | 8 | # The master toctree document. 9 | master_doc = 'index' 10 | 11 | # The suffix of source filenames. 12 | source_suffix = '.rst' 13 | 14 | project = u'rviz_python_tutorial' 15 | 16 | copyright = u'2012, Willow Garage, Inc' 17 | 18 | # If true, sectionauthor and moduleauthor directives will be shown in the 19 | # output. They are ignored by default. 20 | show_authors = True 21 | 22 | # The name of the Pygments (syntax highlighting) style to use. 23 | pygments_style = 'sphinx' 24 | 25 | extlinks = {'srcdir': ('https://github.com/ros-visualization/visualization_tutorials/blob/groovy-devel/rviz_python_tutorial/%s', '')} 26 | -------------------------------------------------------------------------------- /rviz_python_tutorial/doc-src/index.rst: -------------------------------------------------------------------------------- 1 | RViz Python Tutorial 2 | ==================== 3 | 4 | Overview 5 | -------- 6 | 7 | RViz is not just a visualizer application, it is also a Python 8 | library! Much of RViz's functionality can be accessed from Python 9 | code by importing the librviz Python bindings. 10 | 11 | This tutorial shows a simple example of creating a visualizer 12 | (rviz::VisualizationFrame) as a child widget along with other Qt 13 | widgets, programmatically loading a config file, then connecting a 14 | slider and some Qt push buttons to change display a display property 15 | and the viewpoint. 16 | 17 | The source code for this tutorial is in the rviz_python_tutorial 18 | package. You can check out the source directly or (if you use Ubuntu) 19 | you can just apt-get install the pre-compiled Debian package like so:: 20 | 21 | sudo apt-get install ros-groovy-visualization-tutorials 22 | 23 | The running application looks like this: 24 | 25 | .. image:: myviz.png 26 | 27 | The Code: myviz.py 28 | ------------------ 29 | 30 | The full text of myviz.py is here: :srcdir:`myviz.py` 31 | 32 | .. tutorial-formatter:: ../myviz.py 33 | 34 | Running 35 | ------- 36 | 37 | Just type:: 38 | 39 | roscd rviz_python_tutorial 40 | ./myviz.py 41 | 42 | myviz.py loads its config file from the current directory, so you need 43 | to run it from the directory it comes in, or adapt the script to find 44 | the file. 45 | 46 | There are more classes in RViz which do not yet have Python bindings. 47 | If you find important ones are missing, please request them as 48 | "enhancement" issues on the RViz project on github. 49 | -------------------------------------------------------------------------------- /rviz_python_tutorial/doc-src/myviz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-visualization/visualization_tutorials/3141f0dfcb7c34ce57909226d5394ca5fd57a5c7/rviz_python_tutorial/doc-src/myviz.png -------------------------------------------------------------------------------- /rviz_python_tutorial/doc-src/tutorialformatter.py: -------------------------------------------------------------------------------- 1 | """ 2 | tutorialformatter 3 | =========================== 4 | 5 | This extension provides a directive to include a source code file 6 | in a document, but with certain comments from the file formatted 7 | as regular document text. This allows code for a tutorial to look like: 8 | 9 | /// BEGIN_TUTORIAL 10 | /// This next line adds one. 11 | i = i + 1; 12 | /// Then we need to double it. 13 | i = i * 2; 14 | /// END_TUTORIAL 15 | 16 | And have it formatted as 17 | 18 | This next line adds one.:: 19 | i = i + 1; 20 | 21 | Then we need to double it.:: 22 | i = i * 2; 23 | 24 | The special-looking comment character sequence at the start of 25 | each text line can be anything not starting or ending with 26 | whitespace. tutorialformatter starts by scanning the file for the 27 | string BEGIN_TUTORIAL. When it finds it, it takes all the 28 | characters before BEGIN_TUTORIAL on that line, strips whitespace 29 | from the left, and uses that as the text marker. So this would 30 | also be fine: 31 | 32 | #My Tutorial# BEGIN_TUTORIAL 33 | #My Tutorial# This next line adds one. 34 | i = i + 1 35 | #My Tutorial# Then we need to double it. 36 | i = i * 2 37 | #My Tutorial# END_TUTORIAL 38 | 39 | .. moduleauthor:: Dave Hershberger 40 | """ 41 | 42 | __version__ = '0.1.0' 43 | 44 | import os 45 | from docutils.parsers import rst 46 | from docutils.parsers.rst.directives import flag, unchanged 47 | from docutils.statemachine import string2lines 48 | from pygments.lexers import get_lexer_for_filename 49 | 50 | class TutorialFormatterDirective(rst.Directive): 51 | has_content = False 52 | final_argument_whitespace = True 53 | required_arguments = 1 54 | 55 | option_spec = dict(shell=flag, prompt=flag, nostderr=flag, 56 | in_srcdir=flag, extraargs=unchanged, 57 | until=unchanged) 58 | 59 | def run(self): 60 | filename = self.arguments[0] 61 | text_tag = None 62 | tag_len = 0 63 | 64 | filepath = self.state.document.settings.env.srcdir 65 | absfilename = os.path.join( filepath, filename ) 66 | if absfilename.endswith('.h'): 67 | language = 'c++' 68 | elif absfilename.endswith('CMakeLists.txt'): 69 | language = 'cmake' 70 | else: 71 | try: 72 | language = get_lexer_for_filename( absfilename ).name.lower() 73 | if language == 'text only': 74 | language = 'none' 75 | except: 76 | language = 'none' 77 | code_prefix = '\n.. code-block:: ' + language + '\n\n' 78 | code_suffix = '\n' 79 | 80 | print(f"tutorial-formatter running on {absfilename}") 81 | file_ = open( absfilename, 'r' ) 82 | text_to_process = "" 83 | current_block = "" 84 | in_code = False 85 | in_text = False 86 | in_tutorial = False 87 | for line in file_: 88 | if not in_tutorial: 89 | begin_pos = line.find( 'BEGIN_TUTORIAL' ) 90 | if begin_pos != -1: 91 | text_tag = line[:begin_pos].lstrip() 92 | tag_len = len( text_tag ) 93 | in_tutorial = True 94 | continue 95 | if line.find( 'END_TUTORIAL' ) != -1: 96 | break 97 | stripped = line.lstrip() 98 | if stripped.startswith( text_tag.strip() ): 99 | if in_code: 100 | text_to_process += code_prefix + current_block + code_suffix 101 | current_block = "" 102 | in_code = False 103 | in_text = True 104 | addition = stripped[tag_len:] 105 | if addition == '' or addition[-1] != '\n': 106 | addition += '\n' 107 | current_block += addition 108 | else: 109 | if in_text: 110 | text_to_process += current_block 111 | current_block = "" 112 | in_text = False 113 | in_code = True # Code to show begins right after tagged text 114 | if in_code: 115 | current_block += ' ' + line 116 | if in_code: 117 | text_to_process += code_prefix + current_block + code_suffix 118 | elif in_text: 119 | text_to_process += current_block 120 | 121 | # Debug writes... 122 | #print 'text_to_process =' 123 | #print text_to_process 124 | #print '= text_to_process' 125 | 126 | lines = string2lines( text_to_process ) 127 | self.state_machine.insert_input( lines, absfilename ) 128 | 129 | return [] 130 | 131 | def setup(app): 132 | app.add_directive('tutorial-formatter', TutorialFormatterDirective) 133 | -------------------------------------------------------------------------------- /rviz_python_tutorial/myviz.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ## BEGIN_TUTORIAL 4 | ## 5 | ## Imports 6 | ## ^^^^^^^ 7 | ## 8 | ## First we start with the standard ros Python import line: 9 | import roslib; roslib.load_manifest('rviz_python_tutorial') 10 | 11 | ## Then load sys to get sys.argv. 12 | import sys 13 | 14 | ## Next import all the Qt bindings into the current namespace, for 15 | ## convenience. This uses the "python_qt_binding" package which hides 16 | ## differences between PyQt and PySide, and works if at least one of 17 | ## the two is installed. The RViz Python bindings use 18 | ## python_qt_binding internally, so you should use it here as well. 19 | from python_qt_binding.QtGui import * 20 | from python_qt_binding.QtCore import * 21 | try: 22 | from python_qt_binding.QtWidgets import * 23 | except ImportError: 24 | pass 25 | 26 | ## Finally import the RViz bindings themselves. 27 | from rviz import bindings as rviz 28 | 29 | ## The MyViz class is the main container widget. 30 | class MyViz( QWidget ): 31 | 32 | ## MyViz Constructor 33 | ## ^^^^^^^^^^^^^^^^^ 34 | ## 35 | ## Its constructor creates and configures all the component widgets: 36 | ## frame, thickness_slider, top_button, and side_button, and adds them 37 | ## to layouts. 38 | def __init__(self): 39 | QWidget.__init__(self) 40 | 41 | ## rviz.VisualizationFrame is the main container widget of the 42 | ## regular RViz application, with menus, a toolbar, a status 43 | ## bar, and many docked subpanels. In this example, we 44 | ## disable everything so that the only thing visible is the 3D 45 | ## render window. 46 | self.frame = rviz.VisualizationFrame() 47 | 48 | ## The "splash path" is the full path of an image file which 49 | ## gets shown during loading. Setting it to the empty string 50 | ## suppresses that behavior. 51 | self.frame.setSplashPath( "" ) 52 | 53 | ## VisualizationFrame.initialize() must be called before 54 | ## VisualizationFrame.load(). In fact it must be called 55 | ## before most interactions with RViz classes because it 56 | ## instantiates and initializes the VisualizationManager, 57 | ## which is the central class of RViz. 58 | self.frame.initialize() 59 | 60 | ## The reader reads config file data into the config object. 61 | ## VisualizationFrame reads its data from the config object. 62 | reader = rviz.YamlConfigReader() 63 | config = rviz.Config() 64 | reader.readFile( config, "config.myviz" ) 65 | self.frame.load( config ) 66 | 67 | ## You can also store any other application data you like in 68 | ## the config object. Here we read the window title from the 69 | ## map key called "Title", which has been added by hand to the 70 | ## config file. 71 | self.setWindowTitle( config.mapGetChild( "Title" ).getValue() ) 72 | 73 | ## Here we disable the menu bar (from the top), status bar 74 | ## (from the bottom), and the "hide-docks" buttons, which are 75 | ## the tall skinny buttons on the left and right sides of the 76 | ## main render window. 77 | self.frame.setMenuBar( None ) 78 | self.frame.setStatusBar( None ) 79 | self.frame.setHideButtonVisibility( False ) 80 | 81 | ## frame.getManager() returns the VisualizationManager 82 | ## instance, which is a very central class. It has pointers 83 | ## to other manager objects and is generally required to make 84 | ## any changes in an rviz instance. 85 | self.manager = self.frame.getManager() 86 | 87 | ## Since the config file is part of the source code for this 88 | ## example, we know that the first display in the list is the 89 | ## grid we want to control. Here we just save a reference to 90 | ## it for later. 91 | self.grid_display = self.manager.getRootDisplayGroup().getDisplayAt( 0 ) 92 | 93 | ## Here we create the layout and other widgets in the usual Qt way. 94 | layout = QVBoxLayout() 95 | layout.addWidget( self.frame ) 96 | 97 | thickness_slider = QSlider( Qt.Horizontal ) 98 | thickness_slider.setTracking( True ) 99 | thickness_slider.setMinimum( 1 ) 100 | thickness_slider.setMaximum( 1000 ) 101 | thickness_slider.valueChanged.connect( self.onThicknessSliderChanged ) 102 | layout.addWidget( thickness_slider ) 103 | 104 | h_layout = QHBoxLayout() 105 | 106 | top_button = QPushButton( "Top View" ) 107 | top_button.clicked.connect( self.onTopButtonClick ) 108 | h_layout.addWidget( top_button ) 109 | 110 | side_button = QPushButton( "Side View" ) 111 | side_button.clicked.connect( self.onSideButtonClick ) 112 | h_layout.addWidget( side_button ) 113 | 114 | layout.addLayout( h_layout ) 115 | 116 | self.setLayout( layout ) 117 | 118 | ## Handle GUI events 119 | ## ^^^^^^^^^^^^^^^^^ 120 | ## 121 | ## After the constructor, for this example the class just needs to 122 | ## respond to GUI events. Here is the slider callback. 123 | ## rviz.Display is a subclass of rviz.Property. Each Property can 124 | ## have sub-properties, forming a tree. To change a Property of a 125 | ## Display, use the subProp() function to walk down the tree to 126 | ## find the child you need. 127 | def onThicknessSliderChanged( self, new_value ): 128 | if self.grid_display != None: 129 | self.grid_display.subProp( "Line Style" ).subProp( "Line Width" ).setValue( new_value / 1000.0 ) 130 | 131 | ## The view buttons just call switchToView() with the name of a saved view. 132 | def onTopButtonClick( self ): 133 | self.switchToView( "Top View" ); 134 | 135 | def onSideButtonClick( self ): 136 | self.switchToView( "Side View" ); 137 | 138 | ## switchToView() works by looping over the views saved in the 139 | ## ViewManager and looking for one with a matching name. 140 | ## 141 | ## view_man.setCurrentFrom() takes the saved view 142 | ## instance and copies it to set the current view 143 | ## controller. 144 | def switchToView( self, view_name ): 145 | view_man = self.manager.getViewManager() 146 | for i in range( view_man.getNumViews() ): 147 | if view_man.getViewAt( i ).getName() == view_name: 148 | view_man.setCurrentFrom( view_man.getViewAt( i )) 149 | return 150 | print( "Did not find view named %s." % view_name ) 151 | 152 | ## Start the Application 153 | ## ^^^^^^^^^^^^^^^^^^^^^ 154 | ## 155 | ## That is just about it. All that's left is the standard Qt 156 | ## top-level application code: create a QApplication, instantiate our 157 | ## class, and start Qt's main event loop (app.exec_()). 158 | if __name__ == '__main__': 159 | app = QApplication( sys.argv ) 160 | 161 | myviz = MyViz() 162 | myviz.resize( 500, 500 ) 163 | myviz.show() 164 | 165 | app.exec_() 166 | -------------------------------------------------------------------------------- /rviz_python_tutorial/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rviz_python_tutorial 3 | 0.11.2 4 | 5 | Tutorials showing how to call into rviz internals from python scripts. 6 | 7 | Mabel Zhang 8 | BSD 9 | 10 | http://ros.org/wiki/rviz_python_tutorial 11 | Dave Hershberger 12 | 13 | catkin 14 | 15 | rviz 16 | 17 | rviz 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /rviz_python_tutorial/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: sphinx 2 | sphinx_root_dir: doc-src 3 | -------------------------------------------------------------------------------- /visualization_marker_tutorials/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package visualization_marker_tutorials 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.11.2 (2025-04-26) 6 | ------------------- 7 | * Remove '/' (forward slash) from '/my_frame', which frustratingly makes Rviz unable to show the markers. (`#70 `_) 8 | * Contributors: Phase Le 9 | 10 | 0.11.1 (2025-04-10) 11 | ------------------- 12 | * Update maintainers (`#63 `_) 13 | * Contributors: Mabel Zhang 14 | 15 | 0.11.0 (2020-05-13) 16 | ------------------- 17 | 18 | 0.10.4 (2020-05-13) 19 | ------------------- 20 | * Updated required CMake version to avoid CMP0048 warning (`#57 `_) 21 | * Updated to use ``ros::Duration`` for sleeping to gain cross-platform support (`#48 `_) 22 | * Contributors: Alejandro Hernández Cordero, Sean Yen [MSFT] 23 | 24 | 0.10.3 (2018-05-09) 25 | ------------------- 26 | 27 | 0.10.2 (2018-01-05) 28 | ------------------- 29 | 30 | 0.10.1 (2016-04-21) 31 | ------------------- 32 | 33 | 0.10.0 (2016-04-21) 34 | ------------------- 35 | 36 | 0.9.2 (2015-09-21) 37 | ------------------ 38 | 39 | 0.9.1 (2015-01-26) 40 | ------------------ 41 | * Now checks number of subscribers before publishing any markers. 42 | * Documented the new DELETEALL feature. 43 | * Contributors: Dave Coleman, Victor Lamoine 44 | 45 | 0.9.0 (2014-03-24) 46 | ------------------ 47 | * set myself (william) as maintainer 48 | * Contributors: William Woodall 49 | -------------------------------------------------------------------------------- /visualization_marker_tutorials/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(visualization_marker_tutorials) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roscpp visualization_msgs) 5 | 6 | ################################### 7 | ## catkin specific configuration ## 8 | ################################### 9 | ## The catkin_package macro generates cmake config files for your package 10 | ## Declare things to be passed to dependent projects 11 | ## LIBRARIES: libraries you create in this project that dependent projects also need 12 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 13 | ## DEPENDS: system dependencies of this project that dependent projects also need 14 | 15 | catkin_package( 16 | CATKIN_DEPENDS roscpp visualization_msgs 17 | ) 18 | 19 | ########### 20 | ## Build ## 21 | ########### 22 | 23 | include_directories(include 24 | ${catkin_INCLUDE_DIRS} 25 | ) 26 | 27 | add_executable(basic_shapes src/basic_shapes.cpp) 28 | target_link_libraries(basic_shapes 29 | ${catkin_LIBRARIES} 30 | ) 31 | 32 | add_executable(points_and_lines src/points_and_lines.cpp) 33 | target_link_libraries(points_and_lines 34 | ${catkin_LIBRARIES} 35 | ) 36 | 37 | ############# 38 | ## Install ## 39 | ############# 40 | 41 | ## Mark executables and/or libraries for installation 42 | install(TARGETS 43 | basic_shapes 44 | points_and_lines 45 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 46 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 47 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 48 | ) 49 | -------------------------------------------------------------------------------- /visualization_marker_tutorials/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | visualization_marker_tutorials 4 | 0.11.2 5 | The visulalization_marker_tutorials package 6 | 7 | Mabel Zhang 8 | 9 | BSD 10 | http://ros.org/wiki/visualization_marker_tutorials 11 | 12 | Josh Faust 13 | 14 | catkin 15 | 16 | roscpp 17 | visualization_msgs 18 | 19 | roscpp 20 | visualization_msgs 21 | 22 | 23 | -------------------------------------------------------------------------------- /visualization_marker_tutorials/src/basic_shapes.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2010, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | // %Tag(FULLTEXT)% 31 | // %Tag(INCLUDES)% 32 | #include 33 | #include 34 | // %EndTag(INCLUDES)% 35 | 36 | // %Tag(INIT)% 37 | int main( int argc, char** argv ) 38 | { 39 | ros::init(argc, argv, "basic_shapes"); 40 | ros::NodeHandle n; 41 | ros::Rate r(1); 42 | ros::Publisher marker_pub = n.advertise("visualization_marker", 1); 43 | // %EndTag(INIT)% 44 | 45 | // Set our initial shape type to be a cube 46 | // %Tag(SHAPE_INIT)% 47 | uint32_t shape = visualization_msgs::Marker::CUBE; 48 | // %EndTag(SHAPE_INIT)% 49 | 50 | // %Tag(MARKER_INIT)% 51 | while (ros::ok()) 52 | { 53 | visualization_msgs::Marker marker; 54 | // Set the frame ID and timestamp. See the TF tutorials for information on these. 55 | marker.header.frame_id = "my_frame"; 56 | marker.header.stamp = ros::Time::now(); 57 | // %EndTag(MARKER_INIT)% 58 | 59 | // Set the namespace and id for this marker. This serves to create a unique ID 60 | // Any marker sent with the same namespace and id will overwrite the old one 61 | // %Tag(NS_ID)% 62 | marker.ns = "basic_shapes"; 63 | marker.id = 0; 64 | // %EndTag(NS_ID)% 65 | 66 | // Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER 67 | // %Tag(TYPE)% 68 | marker.type = shape; 69 | // %EndTag(TYPE)% 70 | 71 | // Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) 72 | // %Tag(ACTION)% 73 | marker.action = visualization_msgs::Marker::ADD; 74 | // %EndTag(ACTION)% 75 | 76 | // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header 77 | // %Tag(POSE)% 78 | marker.pose.position.x = 0; 79 | marker.pose.position.y = 0; 80 | marker.pose.position.z = 0; 81 | marker.pose.orientation.x = 0.0; 82 | marker.pose.orientation.y = 0.0; 83 | marker.pose.orientation.z = 0.0; 84 | marker.pose.orientation.w = 1.0; 85 | // %EndTag(POSE)% 86 | 87 | // Set the scale of the marker -- 1x1x1 here means 1m on a side 88 | // %Tag(SCALE)% 89 | marker.scale.x = 1.0; 90 | marker.scale.y = 1.0; 91 | marker.scale.z = 1.0; 92 | // %EndTag(SCALE)% 93 | 94 | // Set the color -- be sure to set alpha to something non-zero! 95 | // %Tag(COLOR)% 96 | marker.color.r = 0.0f; 97 | marker.color.g = 1.0f; 98 | marker.color.b = 0.0f; 99 | marker.color.a = 1.0; 100 | // %EndTag(COLOR)% 101 | 102 | // %Tag(LIFETIME)% 103 | marker.lifetime = ros::Duration(); 104 | // %EndTag(LIFETIME)% 105 | 106 | // Publish the marker 107 | // %Tag(PUBLISH)% 108 | while (marker_pub.getNumSubscribers() < 1) 109 | { 110 | if (!ros::ok()) 111 | { 112 | return 0; 113 | } 114 | ROS_WARN_ONCE("Please create a subscriber to the marker"); 115 | ros::Duration(1.0).sleep(); 116 | } 117 | marker_pub.publish(marker); 118 | // %EndTag(PUBLISH)% 119 | 120 | // Cycle between different shapes 121 | // %Tag(CYCLE_SHAPES)% 122 | switch (shape) 123 | { 124 | case visualization_msgs::Marker::CUBE: 125 | shape = visualization_msgs::Marker::SPHERE; 126 | break; 127 | case visualization_msgs::Marker::SPHERE: 128 | shape = visualization_msgs::Marker::ARROW; 129 | break; 130 | case visualization_msgs::Marker::ARROW: 131 | shape = visualization_msgs::Marker::CYLINDER; 132 | break; 133 | case visualization_msgs::Marker::CYLINDER: 134 | shape = visualization_msgs::Marker::CUBE; 135 | break; 136 | } 137 | // %EndTag(CYCLE_SHAPES)% 138 | 139 | // %Tag(SLEEP_END)% 140 | r.sleep(); 141 | } 142 | // %EndTag(SLEEP_END)% 143 | } 144 | // %EndTag(FULLTEXT)% 145 | -------------------------------------------------------------------------------- /visualization_marker_tutorials/src/points_and_lines.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2010, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | // %Tag(FULLTEXT)% 31 | #include 32 | #include 33 | 34 | #include 35 | 36 | int main( int argc, char** argv ) 37 | { 38 | ros::init(argc, argv, "points_and_lines"); 39 | ros::NodeHandle n; 40 | ros::Publisher marker_pub = n.advertise("visualization_marker", 10); 41 | 42 | ros::Rate r(30); 43 | 44 | float f = 0.0; 45 | while (ros::ok()) 46 | { 47 | // %Tag(MARKER_INIT)% 48 | visualization_msgs::Marker points, line_strip, line_list; 49 | points.header.frame_id = line_strip.header.frame_id = line_list.header.frame_id = "my_frame"; 50 | points.header.stamp = line_strip.header.stamp = line_list.header.stamp = ros::Time::now(); 51 | points.ns = line_strip.ns = line_list.ns = "points_and_lines"; 52 | points.action = line_strip.action = line_list.action = visualization_msgs::Marker::ADD; 53 | points.pose.orientation.w = line_strip.pose.orientation.w = line_list.pose.orientation.w = 1.0; 54 | // %EndTag(MARKER_INIT)% 55 | 56 | // %Tag(ID)% 57 | points.id = 0; 58 | line_strip.id = 1; 59 | line_list.id = 2; 60 | // %EndTag(ID)% 61 | 62 | // %Tag(TYPE)% 63 | points.type = visualization_msgs::Marker::POINTS; 64 | line_strip.type = visualization_msgs::Marker::LINE_STRIP; 65 | line_list.type = visualization_msgs::Marker::LINE_LIST; 66 | // %EndTag(TYPE)% 67 | 68 | // %Tag(SCALE)% 69 | // POINTS markers use x and y scale for width/height respectively 70 | points.scale.x = 0.2; 71 | points.scale.y = 0.2; 72 | 73 | // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width 74 | line_strip.scale.x = 0.1; 75 | line_list.scale.x = 0.1; 76 | // %EndTag(SCALE)% 77 | 78 | // %Tag(COLOR)% 79 | // Points are green 80 | points.color.g = 1.0f; 81 | points.color.a = 1.0; 82 | 83 | // Line strip is blue 84 | line_strip.color.b = 1.0; 85 | line_strip.color.a = 1.0; 86 | 87 | // Line list is red 88 | line_list.color.r = 1.0; 89 | line_list.color.a = 1.0; 90 | // %EndTag(COLOR)% 91 | 92 | // %Tag(HELIX)% 93 | // Create the vertices for the points and lines 94 | for (uint32_t i = 0; i < 100; ++i) 95 | { 96 | float y = 5 * sin(f + i / 100.0f * 2 * M_PI); 97 | float z = 5 * cos(f + i / 100.0f * 2 * M_PI); 98 | 99 | geometry_msgs::Point p; 100 | p.x = (int32_t)i - 50; 101 | p.y = y; 102 | p.z = z; 103 | 104 | points.points.push_back(p); 105 | line_strip.points.push_back(p); 106 | 107 | // The line list needs two points for each line 108 | line_list.points.push_back(p); 109 | p.z += 1.0; 110 | line_list.points.push_back(p); 111 | } 112 | // %EndTag(HELIX)% 113 | 114 | marker_pub.publish(points); 115 | marker_pub.publish(line_strip); 116 | marker_pub.publish(line_list); 117 | 118 | r.sleep(); 119 | 120 | f += 0.04; 121 | } 122 | } 123 | // %EndTag(FULLTEXT)% 124 | 125 | -------------------------------------------------------------------------------- /visualization_tutorials/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package visualization_tutorials 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.11.2 (2025-04-26) 6 | ------------------- 7 | 8 | 0.11.1 (2025-04-10) 9 | ------------------- 10 | * Update maintainers (`#63 `_) 11 | * Contributors: Mabel Zhang 12 | 13 | 0.11.0 (2020-05-13) 14 | ------------------- 15 | 16 | 0.10.4 (2020-05-13) 17 | ------------------- 18 | * Updated required CMake version to avoid CMP0048 warning (`#57 `_) 19 | * Contributors: Alejandro Hernández Cordero 20 | 21 | 0.10.3 (2018-05-09) 22 | ------------------- 23 | 24 | 0.10.2 (2018-01-05) 25 | ------------------- 26 | 27 | 0.10.1 (2016-04-21) 28 | ------------------- 29 | 30 | 0.10.0 (2016-04-21) 31 | ------------------- 32 | 33 | 0.9.2 (2015-09-21) 34 | ------------------ 35 | 36 | 0.9.1 (2015-01-26) 37 | ------------------ 38 | 39 | 0.9.0 (2014-03-24) 40 | ------------------ 41 | * set myself (william) as maintainer 42 | * Contributors: William Woodall 43 | -------------------------------------------------------------------------------- /visualization_tutorials/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(visualization_tutorials) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /visualization_tutorials/package.xml: -------------------------------------------------------------------------------- 1 | 2 | visualization_tutorials 3 | 0.11.2 4 | 5 | Metapackage referencing tutorials related to rviz and visualization. 6 | 7 | Mabel Zhang 8 | BSD 9 | 10 | http://ros.org/wiki/visualization_tutorials 11 | Dave Hershberger 12 | David Gossow 13 | Josh Faust 14 | 15 | catkin 16 | 17 | interactive_marker_tutorials 18 | librviz_tutorial 19 | rviz_plugin_tutorials 20 | rviz_python_tutorial 21 | visualization_marker_tutorials 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | --------------------------------------------------------------------------------