[ros-rviz] 01/04: New upstream version 1.12.2+dfsg
Jochen Sprickerhof
jspricke at moszumanska.debian.org
Thu Oct 20 15:12:59 UTC 2016
This is an automated email from the git hooks/post-receive script.
jspricke pushed a commit to branch master
in repository ros-rviz.
commit 89548653f226c98b403a5076524272299d921e21
Author: Jochen Sprickerhof <git at jochen.sprickerhof.de>
Date: Wed Oct 19 09:43:42 2016 +0200
New upstream version 1.12.2+dfsg
---
CHANGELOG.rst | 19 ++
CMakeLists.txt | 28 +--
README.md | 4 +-
package.xml | 6 +-
src/image_view/CMakeLists.txt | 6 +-
src/python_bindings/shiboken/CMakeLists.txt | 3 +
src/rviz/CMakeLists.txt | 27 +-
src/rviz/default_plugin/CMakeLists.txt | 5 +-
src/rviz/default_plugin/depth_cloud_mld.cpp | 2 +-
src/rviz/default_plugin/effort_display.cpp | 6 +-
src/rviz/default_plugin/effort_display.h | 8 +-
src/rviz/default_plugin/effort_visual.cpp | 5 +-
src/rviz/default_plugin/path_display.cpp | 234 +++++++++++++++++
src/rviz/default_plugin/path_display.h | 29 +++
src/rviz/default_plugin/pose_array_display.cpp | 276 +++++++++++++++++++--
src/rviz/default_plugin/pose_array_display.h | 56 ++++-
.../view_controllers/orbit_view_controller.cpp | 32 ++-
.../view_controllers/orbit_view_controller.h | 8 +
src/rviz/display.h | 12 +-
src/rviz/display_group.cpp | 4 +-
src/rviz/displays_panel.cpp | 1 +
src/rviz/ogre_helpers/grid.cpp | 2 +-
src/rviz/ogre_helpers/line.cpp | 2 +-
src/rviz/ogre_helpers/shape.cpp | 2 +-
src/rviz/properties/float_edit.cpp | 8 +-
src/rviz/properties/property.h | 8 +-
src/rviz/render_panel.cpp | 4 +
src/rviz/robot/robot.cpp | 21 +-
src/rviz/robot/robot.h | 14 +-
src/rviz/robot/robot_joint.cpp | 6 +-
src/rviz/robot/robot_joint.h | 15 +-
src/rviz/robot/robot_link.cpp | 35 ++-
src/rviz/robot/robot_link.h | 23 +-
src/rviz/tool.h | 22 +-
src/rviz/tool_manager.cpp | 8 +
src/rviz/tool_manager.h | 3 +
src/rviz/view_controller.h | 26 +-
src/rviz/visualization_manager.cpp | 4 +-
src/rviz/visualizer_app.cpp | 2 +-
src/test/CMakeLists.txt | 85 ++++---
40 files changed, 871 insertions(+), 190 deletions(-)
diff --git a/CHANGELOG.rst b/CHANGELOG.rst
index 96f0be6..a46c144 100644
--- a/CHANGELOG.rst
+++ b/CHANGELOG.rst
@@ -2,6 +2,25 @@
Changelog for package rviz
^^^^^^^^^^^^^^^^^^^^^^^^^^
+1.12.2 (2016-10-18)
+-------------------
+* Paths can now be rendered as 3D arrows or pose markers (`#1059 <https://github.com/ros-visualization/rviz/issues/1059>`_)
+* Allow float edits to work with different Locales (`#1043 <https://github.com/ros-visualization/rviz/issues/1043>`_)
+* Now check for a valid root link before walking the robot model (`#1041 <https://github.com/ros-visualization/rviz/issues/1041>`_)
+* Added close() signal to Tool class (`#1051 <https://github.com/ros-visualization/rviz/issues/1051>`_)
+* Fix double free in display dialog (`#1053 <https://github.com/ros-visualization/rviz/issues/1053>`_)
+* Tweak focal shape size marker depending on focal distance (`#1021 <https://github.com/ros-visualization/rviz/issues/1021>`_)
+* Support 3D arrows and axes for visualizing PoseArrays (`#1022 <https://github.com/ros-visualization/rviz/issues/1022>`_)
+* Use urdf::*ShredPtr instead of boost::shared_ptr (`#1044 <https://github.com/ros-visualization/rviz/issues/1044>`_)
+* Fixed two valgrind-reported issues (`#1027 <https://github.com/ros-visualization/rviz/issues/1027>`_)
+ * in ~RenderPanel()
+ * in VisualizationManager(): initialization order
+* Added option to disable the RViz splash-screen (`#1024 <https://github.com/ros-visualization/rviz/issues/1024>`_)
+* Fix compile error due to the user-defined string literals feature (`#1010 <https://github.com/ros-visualization/rviz/issues/1010>`_)
+* Fixed some Qt5 related build issues (`#1008 <https://github.com/ros-visualization/rviz/issues/1008>`_)
+* Removed dependency on OpenCV (`#1009 <https://github.com/ros-visualization/rviz/issues/1009>`_)
+* Contributors: 1r0b1n0, Atsushi Watanabe, Blake Anderson, Jochen Sprickerhof, Kartik Mohta, Maarten de Vries, Michael Görner, Robert Haschke, Victor Lamoine, Víctor Mayoral Vilches, William Woodall
+
1.12.1 (2016-04-20)
-------------------
* Updated the ``plugin_description.xml`` to reflect the new default plugin library name, see: `#1004 <https://github.com/ros-visualization/rviz/issues/1004>`_
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 640e9c3..597aecb 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -17,6 +17,8 @@ find_package(Boost REQUIRED
thread
)
+find_package(urdfdom_headers REQUIRED)
+
find_package(PkgConfig REQUIRED)
find_package(ASSIMP QUIET)
@@ -96,19 +98,21 @@ set(CMAKE_INCLUDE_CURRENT_DIR ON)
option(UseQt5 "UseQt5" ON)
if (UseQt5)
- find_package(Qt5Widgets REQUIRED)
- find_package(Qt5Core REQUIRED)
- find_package(Qt5OpenGL REQUIRED)
+ find_package(Qt5 REQUIRED COMPONENTS Core Widgets OpenGL)
+ # set variable names already used with Qt4
+ set(QT_LIBRARIES Qt5::Widgets)
+ set(QTVERSION ${Qt5Widgets_VERSION})
else()
find_package(Qt4 REQUIRED COMPONENTS QtCore QtGui QtOpenGL)
+ include(${QT_USE_FILE})
endif()
+add_definitions(-DQT_NO_KEYWORDS)
find_package(catkin REQUIRED
COMPONENTS
angles
cmake_modules
geometry_msgs
- image_geometry
image_transport
interactive_markers
laser_geometry
@@ -129,6 +133,7 @@ find_package(catkin REQUIRED
tf
urdf
visualization_msgs
+ urdfdom_headers
)
if(${tf_VERSION} VERSION_LESS "1.11.3")
@@ -137,7 +142,7 @@ endif()
find_package(PythonLibs "${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}" REQUIRED)
-find_package(Eigen3)
+find_package(Eigen3 QUIET)
if(NOT EIGEN3_FOUND)
# Fallback to cmake_modules
find_package(cmake_modules REQUIRED)
@@ -155,17 +160,8 @@ if(NOT OGRE_OV_LIBRARIES_ABS)
set(OGRE_OV_LIBRARIES_ABS ${OGRE_OV_LIBRARIES})
endif()
-if(NOT UseQt5)
- include(${QT_USE_FILE})
-endif()
-add_definitions(-DQT_NO_KEYWORDS)
-
# Set the Qt version for use in the extras file.
-if(UseQt5)
- set(rviz_QT_VERSION ${Qt5Widgets_VERSION})
-else()
- set(rviz_QT_VERSION ${QTVERSION})
-endif()
+set(rviz_QT_VERSION ${QTVERSION})
# This variable controls the target name of the default plugin library.
# It is used in the extras file (processed in caktin_package(...)) and the
@@ -188,7 +184,6 @@ catkin_package(
${OPENGL_LIBRARIES}
CATKIN_DEPENDS
geometry_msgs
- image_geometry
image_transport
interactive_markers
laser_geometry
@@ -211,6 +206,7 @@ include_directories(SYSTEM
${OGRE_OV_INCLUDE_DIRS}
${OPENGL_INCLUDE_DIR}
${PYTHON_INCLUDE_PATH}
+ ${urdfdom_headers_INCLUDE_DIRS}
)
include_directories(src ${catkin_INCLUDE_DIRS})
diff --git a/README.md b/README.md
index 6a58f36..2d0dfb1 100644
--- a/README.md
+++ b/README.md
@@ -1,6 +1,6 @@

-[](https://travis-ci.org/ros-visualization/rviz)
+[](http://build.ros.org/job/Kpr__rviz__ubuntu_xenial_amd64)
-rviz is a 3D visualiazer for the Robot Operating System (ROS) framework.
+rviz is a 3D visualizer for the Robot Operating System (ROS) framework.
For more information, please see the wiki: http://wiki.ros.org/rviz
diff --git a/package.xml b/package.xml
index c241f29..85ac004 100644
--- a/package.xml
+++ b/package.xml
@@ -1,6 +1,6 @@
<package>
<name>rviz</name>
- <version>1.12.1</version>
+ <version>1.12.2</version>
<description>
3D visualization tool for ROS.
</description>
@@ -22,7 +22,6 @@
<build_depend>cmake_modules</build_depend>
<build_depend>eigen</build_depend>
<build_depend>geometry_msgs</build_depend>
- <build_depend>image_geometry</build_depend>
<build_depend>image_transport</build_depend>
<build_depend>interactive_markers</build_depend>
<build_depend>laser_geometry</build_depend>
@@ -49,11 +48,11 @@
<build_depend>visualization_msgs</build_depend>
<build_depend>yaml-cpp</build_depend>
<build_depend>opengl</build_depend>
+ <build_depend>liburdfdom-headers-dev</build_depend>
<run_depend>assimp</run_depend>
<run_depend>eigen</run_depend>
<run_depend>geometry_msgs</run_depend>
- <run_depend>image_geometry</run_depend>
<run_depend>image_transport</run_depend>
<run_depend>interactive_markers</run_depend>
<run_depend>laser_geometry</run_depend>
@@ -83,6 +82,7 @@
<run_depend>visualization_msgs</run_depend>
<run_depend>yaml-cpp</run_depend>
<run_depend>opengl</run_depend>
+ <run_depend>liburdfdom-headers-dev</run_depend>
<export>
<rviz plugin="${prefix}/plugin_description.xml"/>
diff --git a/src/image_view/CMakeLists.txt b/src/image_view/CMakeLists.txt
index 3760430..331e368 100644
--- a/src/image_view/CMakeLists.txt
+++ b/src/image_view/CMakeLists.txt
@@ -1,9 +1,10 @@
-
add_executable(rviz_image_view
image_view.cpp
main.cpp
- ${MOC_FILES}
)
+if(NOT WIN32)
+ set_target_properties(rviz_image_view PROPERTIES COMPILE_FLAGS "-std=c++11")
+endif()
target_link_libraries(rviz_image_view
${OGRE_OV_LIBRARIES_ABS}
@@ -14,4 +15,3 @@ target_link_libraries(rviz_image_view
set_target_properties(rviz_image_view
PROPERTIES OUTPUT_NAME image_view
PREFIX "")
-
diff --git a/src/python_bindings/shiboken/CMakeLists.txt b/src/python_bindings/shiboken/CMakeLists.txt
index c8a5b8d..f787fb6 100644
--- a/src/python_bindings/shiboken/CMakeLists.txt
+++ b/src/python_bindings/shiboken/CMakeLists.txt
@@ -89,6 +89,9 @@ if(shiboken_helper_FOUND)
)
shiboken_include_directories(rviz_shiboken "${rviz_shiboken_QT_COMPONENTS}")
add_library(rviz_shiboken ${rviz_shiboken_SRCS})
+ if(NOT WIN32)
+ set_target_properties(rviz_shiboken PROPERTIES COMPILE_FLAGS "-std=c++11")
+ endif()
set_target_properties(rviz_shiboken PROPERTIES
LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${PYTHON_INSTALL_DIR}/rviz)
target_link_libraries(rviz_shiboken ${PROJECT_NAME})
diff --git a/src/rviz/CMakeLists.txt b/src/rviz/CMakeLists.txt
index a91330b..3ccaf0e 100644
--- a/src/rviz/CMakeLists.txt
+++ b/src/rviz/CMakeLists.txt
@@ -4,6 +4,10 @@ if(NEW_YAMLCPP_FOUND)
add_definitions(-DRVIZ_HAVE_YAMLCPP_05)
endif(NEW_YAMLCPP_FOUND)
+if(UNIX AND NOT APPLE)
+ find_package(X11 REQUIRED)
+endif()
+
add_subdirectory(default_plugin)
include_directories(.)
@@ -19,7 +23,7 @@ configure_file(env_config.cpp.in ${ENV_CONFIG_FILE} @ONLY)
# We create one lib with the C++...
-add_library( ${PROJECT_NAME}
+add_library(${PROJECT_NAME}
bit_allocator.cpp
config.cpp
display.cpp
@@ -122,23 +126,23 @@ add_library( ${PROJECT_NAME}
yaml_config_writer.cpp
${ENV_CONFIG_FILE}
- ${MOC_FILES}
)
+if(NOT WIN32)
+ set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS "-std=c++11")
+endif()
+
target_link_libraries(${PROJECT_NAME}
${Boost_LIBRARIES}
${catkin_LIBRARIES}
+ ${QT_LIBRARIES}
${OGRE_OV_LIBRARIES_ABS}
${OPENGL_LIBRARIES}
${rviz_ADDITIONAL_LIBRARIES}
+ ${X11_X11_LIB}
assimp
yaml-cpp
)
-if(UseQt5)
- target_link_libraries(${PROJECT_NAME} Qt5::Widgets)
-else()
- target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES})
-endif()
if(APPLE)
@@ -148,13 +152,12 @@ endif(APPLE)
########### The rviz executable ###########
add_executable(executable main.cpp)
-target_link_libraries(executable ${PROJECT_NAME} ${OGRE_OV_LIBRARIES_ABS})
-if(UseQt5)
- target_link_libraries(executable Qt5::Widgets)
-else()
- target_link_libraries(executable ${QT_LIBRARIES})
+if(NOT WIN32)
+ set_target_properties(executable PROPERTIES COMPILE_FLAGS "-std=c++11")
endif()
+target_link_libraries(executable ${PROJECT_NAME} ${QT_LIBRARIES} ${OGRE_OV_LIBRARIES_ABS})
+
set_target_properties(executable
PROPERTIES OUTPUT_NAME ${PROJECT_NAME})
diff --git a/src/rviz/default_plugin/CMakeLists.txt b/src/rviz/default_plugin/CMakeLists.txt
index 198e2e2..fe09627 100644
--- a/src/rviz/default_plugin/CMakeLists.txt
+++ b/src/rviz/default_plugin/CMakeLists.txt
@@ -63,10 +63,13 @@ set(SOURCE_FILES
view_controllers/fps_view_controller.cpp
wrench_display.cpp
wrench_visual.cpp
- ${MOC_FILES}
)
add_library(${rviz_DEFAULT_PLUGIN_LIBRARY_TARGET_NAME} ${SOURCE_FILES})
+if(NOT WIN32)
+ set_target_properties(${rviz_DEFAULT_PLUGIN_LIBRARY_TARGET_NAME}
+ PROPERTIES COMPILE_FLAGS "-std=c++11")
+endif()
target_link_libraries(${rviz_DEFAULT_PLUGIN_LIBRARY_TARGET_NAME}
${PROJECT_NAME}
${Boost_LIBRARIES}
diff --git a/src/rviz/default_plugin/depth_cloud_mld.cpp b/src/rviz/default_plugin/depth_cloud_mld.cpp
index 3035f47..b185c73 100644
--- a/src/rviz/default_plugin/depth_cloud_mld.cpp
+++ b/src/rviz/default_plugin/depth_cloud_mld.cpp
@@ -31,7 +31,7 @@
* Author: jkammerl
*/
-#include <image_geometry/pinhole_camera_model.h>
+#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/image_encodings.h>
#include <string.h>
diff --git a/src/rviz/default_plugin/effort_display.cpp b/src/rviz/default_plugin/effort_display.cpp
index a5574de..e9b595b 100644
--- a/src/rviz/default_plugin/effort_display.cpp
+++ b/src/rviz/default_plugin/effort_display.cpp
@@ -208,11 +208,11 @@ namespace rviz
return;
}
setStatus(rviz::StatusProperty::Ok, "URDF", "Robot model parserd Ok");
- for (std::map<std::string, boost::shared_ptr<urdf::Joint> >::iterator it = robot_model_->joints_.begin(); it != robot_model_->joints_.end(); it ++ ) {
- boost::shared_ptr<urdf::Joint> joint = it->second;
+ for (std::map<std::string, urdf::JointSharedPtr >::iterator it = robot_model_->joints_.begin(); it != robot_model_->joints_.end(); it ++ ) {
+ urdf::JointSharedPtr joint = it->second;
if ( joint->type == urdf::Joint::REVOLUTE ) {
std::string joint_name = it->first;
- boost::shared_ptr<urdf::JointLimits> limit = joint->limits;
+ urdf::JointLimitsSharedPtr limit = joint->limits;
joints_[joint_name] = createJoint(joint_name);
//joints_[joint_name]->max_effort_property_->setFloat(limit->effort);
//joints_[joint_name]->max_effort_property_->setReadOnly( true );
diff --git a/src/rviz/default_plugin/effort_display.h b/src/rviz/default_plugin/effort_display.h
index bc89bd6..c408421 100644
--- a/src/rviz/default_plugin/effort_display.h
+++ b/src/rviz/default_plugin/effort_display.h
@@ -36,13 +36,13 @@ namespace tf
# undef TF_MESSAGEFILTER_DEBUG
#endif
#define TF_MESSAGEFILTER_DEBUG(fmt, ...) \
- ROS_DEBUG_NAMED("message_filter", "MessageFilter [target=%s]: "fmt, getTargetFramesString().c_str(), __VA_ARGS__)
+ ROS_DEBUG_NAMED("message_filter", "MessageFilter [target=%s]: " fmt, getTargetFramesString().c_str(), __VA_ARGS__)
#ifdef TF_MESSAGEFILTER_WARN
# undef TF_MESSAGEFILTER_WARN
#endif
#define TF_MESSAGEFILTER_WARN(fmt, ...) \
- ROS_WARN_NAMED("message_filter", "MessageFilter [target=%s]: "fmt, getTargetFramesString().c_str(), __VA_ARGS__)
+ ROS_WARN_NAMED("message_filter", "MessageFilter [target=%s]: " fmt, getTargetFramesString().c_str(), __VA_ARGS__)
class MessageFilterJointState : public MessageFilter<sensor_msgs::JointState>
{
@@ -127,8 +127,8 @@ public:
clear();
TF_MESSAGEFILTER_DEBUG("Successful Transforms: %llu, Failed Transforms: %llu, Discarded due to age: %llu, Transform messages received: %llu, Messages received: %llu, Total dropped: %llu",
- (long long unsigned int)successful_transform_count_, (long long unsigned int)failed_transform_count_,
- (long long unsigned int)failed_out_the_back_count_, (long long unsigned int)transform_message_count_,
+ (long long unsigned int)successful_transform_count_, (long long unsigned int)failed_transform_count_,
+ (long long unsigned int)failed_out_the_back_count_, (long long unsigned int)transform_message_count_,
(long long unsigned int)incoming_message_count_, (long long unsigned int)dropped_message_count_);
}
diff --git a/src/rviz/default_plugin/effort_visual.cpp b/src/rviz/default_plugin/effort_visual.cpp
index c33716e..922110b 100644
--- a/src/rviz/default_plugin/effort_visual.cpp
+++ b/src/rviz/default_plugin/effort_visual.cpp
@@ -8,6 +8,7 @@
#include <ros/ros.h>
#include <urdf/model.h>
+#include <urdf_model/types.h>
#include "effort_visual.h"
namespace rviz
@@ -31,7 +32,7 @@ namespace rviz
// We create the arrow object within the frame node so that we can
// set its position and direction relative to its header frame.
- for (std::map<std::string, boost::shared_ptr<urdf::Joint> >::iterator it = urdf_model_->joints_.begin(); it != urdf_model_->joints_.end(); it ++ ) {
+ for (std::map<std::string, urdf::JointSharedPtr >::iterator it = urdf_model_->joints_.begin(); it != urdf_model_->joints_.end(); it ++ ) {
if ( it->second->type == urdf::Joint::REVOLUTE ) {
std::string joint_name = it->first;
effort_enabled_[joint_name] = true;
@@ -103,7 +104,7 @@ namespace rviz
if ( ! effort_enabled_[joint_name] ) continue;
//tf::Transform offset = poseFromJoint(joint);
- boost::shared_ptr<urdf::JointLimits> limit = joint->limits;
+ urdf::JointLimitsSharedPtr limit = joint->limits;
double max_effort = limit->effort, effort_value = 0.05;
if ( max_effort != 0.0 )
diff --git a/src/rviz/default_plugin/path_display.cpp b/src/rviz/default_plugin/path_display.cpp
index d18957e..425a3cb 100644
--- a/src/rviz/default_plugin/path_display.cpp
+++ b/src/rviz/default_plugin/path_display.cpp
@@ -82,11 +82,54 @@ PathDisplay::PathDisplay()
offset_property_ = new VectorProperty( "Offset", Ogre::Vector3::ZERO,
"Allows you to offset the path from the origin of the reference frame. In meters.",
this, SLOT( updateOffset() ));
+
+ pose_style_property_ = new EnumProperty( "Pose Style", "None", "Shape to display the pose as.",
+ this, SLOT( updatePoseStyle() ));
+ pose_style_property_->addOption( "None", NONE );
+ pose_style_property_->addOption( "Axes", AXES );
+ pose_style_property_->addOption( "Arrows", ARROWS );
+
+ pose_axes_length_property_ = new rviz::FloatProperty( "Length", 0.3,
+ "Length of the axes.",
+ this, SLOT(updatePoseAxisGeometry()) );
+ pose_axes_radius_property_ = new rviz::FloatProperty( "Radius", 0.03,
+ "Radius of the axes.",
+ this, SLOT(updatePoseAxisGeometry()) );
+
+ pose_arrow_color_property_ = new ColorProperty( "Color",
+ QColor( 255, 85, 255 ),
+ "Color to draw the poses.",
+ this, SLOT(updatePoseArrowColor()));
+ pose_arrow_shaft_length_property_ = new rviz::FloatProperty( "Shaft Length", 0.1,
+ "Length of the arrow shaft.",
+ this,
+ SLOT(updatePoseArrowGeometry()));
+ pose_arrow_head_length_property_ = new rviz::FloatProperty( "Head Length", 0.2,
+ "Length of the arrow head.",
+ this,
+ SLOT(updatePoseArrowGeometry()));
+ pose_arrow_shaft_diameter_property_ = new rviz::FloatProperty( "Shaft Diameter", 0.1,
+ "Diameter of the arrow shaft.",
+ this,
+ SLOT(updatePoseArrowGeometry()));
+ pose_arrow_head_diameter_property_ = new rviz::FloatProperty( "Head Diameter", 0.3,
+ "Diameter of the arrow head.",
+ this,
+ SLOT(updatePoseArrowGeometry()));
+ pose_axes_length_property_->hide();
+ pose_axes_radius_property_->hide();
+ pose_arrow_color_property_->hide();
+ pose_arrow_shaft_length_property_->hide();
+ pose_arrow_head_length_property_->hide();
+ pose_arrow_shaft_diameter_property_->hide();
+ pose_arrow_head_diameter_property_->hide();
}
PathDisplay::~PathDisplay()
{
destroyObjects();
+ destroyPoseAxesChain();
+ destroyPoseArrowChain();
}
void PathDisplay::onInitialize()
@@ -102,6 +145,58 @@ void PathDisplay::reset()
}
+void PathDisplay::allocateAxesVector(std::vector<rviz::Axes*>& axes_vect, int num)
+{
+ if (num > axes_vect.size()) {
+ for (size_t i = axes_vect.size(); i < num; i++) {
+ rviz::Axes* axes = new rviz::Axes( scene_manager_, scene_node_,
+ pose_axes_length_property_->getFloat(),
+ pose_axes_radius_property_->getFloat());
+ axes_vect.push_back(axes);
+ }
+ }
+ else if (num < axes_vect.size()) {
+ for (int i = axes_vect.size() - 1; num <= i; i--) {
+ delete axes_vect[i];
+ }
+ axes_vect.resize(num);
+ }
+}
+
+void PathDisplay::allocateArrowVector(std::vector<rviz::Arrow*>& arrow_vect, int num)
+{
+ if (num > arrow_vect.size()) {
+ for (size_t i = arrow_vect.size(); i < num; i++) {
+ rviz::Arrow* arrow = new rviz::Arrow( scene_manager_, scene_node_ );
+ arrow_vect.push_back(arrow);
+ }
+ }
+ else if (num < arrow_vect.size()) {
+ for (int i = arrow_vect.size() - 1; num <= i; i--) {
+ delete arrow_vect[i];
+ }
+ arrow_vect.resize(num);
+ }
+}
+
+void PathDisplay::destroyPoseAxesChain()
+{
+ for( size_t i = 0; i < axes_chain_.size(); i++ )
+ {
+ allocateAxesVector(axes_chain_[i], 0);
+ }
+ axes_chain_.resize(0);
+}
+
+void PathDisplay::destroyPoseArrowChain()
+{
+ for( size_t i = 0; i < arrow_chain_.size(); i++ )
+ {
+ allocateArrowVector(arrow_chain_[i], 0);
+ }
+ arrow_chain_.resize(0);
+}
+
void PathDisplay::updateStyle()
{
LineStyle style = (LineStyle) style_property_->getOptionInt();
@@ -142,6 +237,86 @@ void PathDisplay::updateOffset()
context_->queueRender();
}
+void PathDisplay::updatePoseStyle()
+{
+ PoseStyle pose_style = (PoseStyle) pose_style_property_->getOptionInt();
+ switch (pose_style)
+ {
+ case AXES:
+ pose_axes_length_property_->show();
+ pose_axes_radius_property_->show();
+ pose_arrow_color_property_->hide();
+ pose_arrow_shaft_length_property_->hide();
+ pose_arrow_head_length_property_->hide();
+ pose_arrow_shaft_diameter_property_->hide();
+ pose_arrow_head_diameter_property_->hide();
+ break;
+ case ARROWS:
+ pose_axes_length_property_->hide();
+ pose_axes_radius_property_->hide();
+ pose_arrow_color_property_->show();
+ pose_arrow_shaft_length_property_->show();
+ pose_arrow_head_length_property_->show();
+ pose_arrow_shaft_diameter_property_->show();
+ pose_arrow_head_diameter_property_->show();
+ break;
+ default:
+ pose_axes_length_property_->hide();
+ pose_axes_radius_property_->hide();
+ pose_arrow_color_property_->hide();
+ pose_arrow_shaft_length_property_->hide();
+ pose_arrow_head_length_property_->hide();
+ pose_arrow_shaft_diameter_property_->hide();
+ pose_arrow_head_diameter_property_->hide();
+ }
+ updateBufferLength();
+}
+
+void PathDisplay::updatePoseAxisGeometry()
+{
+ for(size_t i = 0; i < axes_chain_.size() ; i++)
+ {
+ std::vector<rviz::Axes*>& axes_vect = axes_chain_[i];
+ for(size_t j = 0; j < axes_vect.size() ; j++)
+ {
+ axes_vect[j]->set( pose_axes_length_property_->getFloat(),
+ pose_axes_radius_property_->getFloat() );
+ }
+ }
+ context_->queueRender();
+}
+
+void PathDisplay::updatePoseArrowColor()
+{
+ QColor color = pose_arrow_color_property_->getColor();
+
+ for( size_t i = 0; i < arrow_chain_.size(); i++ )
+ {
+ std::vector<rviz::Arrow*>& arrow_vect = arrow_chain_[i];
+ for( size_t j = 0; j < arrow_vect.size(); j++ )
+ {
+ arrow_vect[j]->setColor( color.redF(), color.greenF(), color.blueF(), 1.0f );
+ }
+ }
+ context_->queueRender();
+}
+
+void PathDisplay::updatePoseArrowGeometry()
+{
+ for( size_t i = 0; i < arrow_chain_.size(); i++ )
+ {
+ std::vector<rviz::Arrow*>& arrow_vect = arrow_chain_[i];
+ for( size_t j = 0; j < arrow_vect.size(); j++ )
+ {
+ arrow_vect[j]->set(pose_arrow_shaft_length_property_->getFloat(),
+ pose_arrow_shaft_diameter_property_->getFloat(),
+ pose_arrow_head_length_property_->getFloat(),
+ pose_arrow_head_diameter_property_->getFloat());
+ }
+ }
+ context_->queueRender();
+}
+
void PathDisplay::destroyObjects()
{
// Destroy all simple lines, if any
@@ -173,6 +348,10 @@ void PathDisplay::updateBufferLength()
// Delete old path objects
destroyObjects();
+ // Destroy all axes and arrows
+ destroyPoseAxesChain();
+ destroyPoseArrowChain();
+
// Read options
int buffer_length = buffer_length_property_->getInt();
LineStyle style = (LineStyle) style_property_->getOptionInt();
@@ -201,6 +380,8 @@ void PathDisplay::updateBufferLength()
}
break;
}
+ axes_chain_.resize( buffer_length );
+ arrow_chain_.resize( buffer_length );
}
@@ -293,6 +474,59 @@ void PathDisplay::processMessage( const nav_msgs::Path::ConstPtr& msg )
break;
}
+ // process pose markers
+ PoseStyle pose_style = (PoseStyle) pose_style_property_->getOptionInt();
+ std::vector<rviz::Arrow*>& arrow_vect = arrow_chain_[ bufferIndex ];
+ std::vector<rviz::Axes*>& axes_vect = axes_chain_[ bufferIndex ];
+
+ switch(pose_style)
+ {
+ case AXES:
+ allocateAxesVector(axes_vect, num_points);
+ for( uint32_t i=0; i < num_points; ++i)
+ {
+ const geometry_msgs::Point& pos = msg->poses[ i ].pose.position;
+ Ogre::Vector3 xpos = transform * Ogre::Vector3( pos.x, pos.y, pos.z );
+ axes_vect[i]->setPosition(xpos);
+ Ogre::Quaternion orientation(msg->poses[ i ].pose.orientation.w,
+ msg->poses[ i ].pose.orientation.x,
+ msg->poses[ i ].pose.orientation.y,
+ msg->poses[ i ].pose.orientation.z);
+ axes_vect[i]->setOrientation(orientation);
+ }
+ break;
+
+ case ARROWS:
+ allocateArrowVector(arrow_vect, num_points);
+ for( uint32_t i=0; i < num_points; ++i)
+ {
+ const geometry_msgs::Point& pos = msg->poses[ i ].pose.position;
+ Ogre::Vector3 xpos = transform * Ogre::Vector3( pos.x, pos.y, pos.z );
+
+ QColor color = pose_arrow_color_property_->getColor();
+ arrow_vect[i]->setColor( color.redF(), color.greenF(), color.blueF(), 1.0f );
+
+ arrow_vect[i]->set(pose_arrow_shaft_length_property_->getFloat(),
+ pose_arrow_shaft_diameter_property_->getFloat(),
+ pose_arrow_head_length_property_->getFloat(),
+ pose_arrow_head_diameter_property_->getFloat());
+ arrow_vect[i]->setPosition(xpos);
+ Ogre::Quaternion orientation(msg->poses[ i ].pose.orientation.w,
+ msg->poses[ i ].pose.orientation.x,
+ msg->poses[ i ].pose.orientation.y,
+ msg->poses[ i ].pose.orientation.z);
+
+ Ogre::Vector3 dir(1, 0, 0);
+ dir = orientation * dir;
+ arrow_vect[i]->setDirection(dir);
+ }
+ break;
+
+ default:
+ break;
+ }
+ context_->queueRender();
+
}
} // namespace rviz
diff --git a/src/rviz/default_plugin/path_display.h b/src/rviz/default_plugin/path_display.h
index 35d9481..33d092d 100644
--- a/src/rviz/default_plugin/path_display.h
+++ b/src/rviz/default_plugin/path_display.h
@@ -34,6 +34,8 @@
#include <nav_msgs/Path.h>
#include "rviz/message_filter_display.h"
+#include <rviz/ogre_helpers/arrow.h>
+#include <rviz/ogre_helpers/axes.h>
namespace Ogre
{
@@ -77,12 +79,22 @@ private Q_SLOTS:
void updateStyle();
void updateLineWidth();
void updateOffset();
+ void updatePoseStyle();
+ void updatePoseAxisGeometry();
+ void updatePoseArrowColor();
+ void updatePoseArrowGeometry();
private:
void destroyObjects();
+ void allocateArrowVector(std::vector<rviz::Arrow*>& arrow_vect, int num);
+ void allocateAxesVector(std::vector<rviz::Axes*>& axes_vect, int num);
+ void destroyPoseAxesChain();
+ void destroyPoseArrowChain();
std::vector<Ogre::ManualObject*> manual_objects_;
std::vector<rviz::BillboardLine*> billboard_lines_;
+ std::vector<std::vector<rviz::Axes*> >axes_chain_;
+ std::vector<std::vector<rviz::Arrow*> >arrow_chain_;
EnumProperty* style_property_;
ColorProperty* color_property_;
@@ -96,6 +108,23 @@ private:
BILLBOARDS
};
+ // pose marker property
+ EnumProperty* pose_style_property_;
+ FloatProperty* pose_axes_length_property_;
+ FloatProperty* pose_axes_radius_property_;
+ ColorProperty* pose_arrow_color_property_;
+ FloatProperty* pose_arrow_shaft_length_property_;
+ FloatProperty* pose_arrow_head_length_property_;
+ FloatProperty* pose_arrow_shaft_diameter_property_;
+ FloatProperty* pose_arrow_head_diameter_property_;
+
+ enum PoseStyle
+ {
+ NONE,
+ AXES,
+ ARROWS,
+ };
+
};
} // namespace rviz
diff --git a/src/rviz/default_plugin/pose_array_display.cpp b/src/rviz/default_plugin/pose_array_display.cpp
index ff4ac9c..ddeaa84 100644
--- a/src/rviz/default_plugin/pose_array_display.cpp
+++ b/src/rviz/default_plugin/pose_array_display.cpp
@@ -33,20 +33,79 @@
#include "rviz/display_context.h"
#include "rviz/frame_manager.h"
+#include "rviz/properties/enum_property.h"
#include "rviz/properties/color_property.h"
#include "rviz/properties/float_property.h"
#include "rviz/validate_floats.h"
+#include "rviz/ogre_helpers/arrow.h"
+#include "rviz/ogre_helpers/axes.h"
#include "rviz/default_plugin/pose_array_display.h"
namespace rviz
{
+namespace
+{
+ struct ShapeType
+ {
+ enum
+ {
+ Arrow2d,
+ Arrow3d,
+ Axes,
+ };
+ };
+
+ Ogre::Vector3 vectorRosToOgre( geometry_msgs::Point const & point )
+ {
+ return Ogre::Vector3( point.x, point.y, point.z );
+ }
+
+ Ogre::Quaternion quaternionRosToOgre( geometry_msgs::Quaternion const & quaternion )
+ {
+ return Ogre::Quaternion( quaternion.w, quaternion.x, quaternion.y, quaternion.z );
+ }
+}
+
PoseArrayDisplay::PoseArrayDisplay()
: manual_object_( NULL )
{
- color_property_ = new ColorProperty( "Color", QColor( 255, 25, 0 ), "Color to draw the arrows.", this );
- length_property_ = new FloatProperty( "Arrow Length", 0.3, "Length of the arrows.", this );
+ shape_property_ = new EnumProperty( "Shape", "Arrow (Flat)", "Shape to display the pose as.",
+ this, SLOT( updateShapeChoice() ) );
+
+ arrow_color_property_ = new ColorProperty( "Color", QColor( 255, 25, 0 ), "Color to draw the arrows.",
+ this, SLOT( updateArrowColor() ) );
+
+ arrow_alpha_property_ = new FloatProperty( "Alpha", 1, "Amount of transparency to apply to the displayed poses.",
+ this, SLOT( updateArrowColor() ) );
+
+ arrow2d_length_property_ = new FloatProperty( "Arrow Length", 0.3, "Length of the arrows.",
+ this, SLOT( updateArrow2dGeometry() ) );
+
+ arrow3d_head_radius_property_ = new FloatProperty( "Head Radius", 0.03, "Radius of the arrow's head, in meters.",
+ this, SLOT( updateArrow3dGeometry() ) );
+
+ arrow3d_head_length_property_ = new FloatProperty( "Head Length", 0.07, "Length of the arrow's head, in meters.",
+ this, SLOT( updateArrow3dGeometry() ) );
+
+ arrow3d_shaft_radius_property_ = new FloatProperty( "Shaft Radius", 0.01, "Radius of the arrow's shaft, in meters.",
+ this, SLOT( updateArrow3dGeometry() ) );
+
+ arrow3d_shaft_length_property_ = new FloatProperty( "Shaft Length", 0.23, "Length of the arrow's shaft, in meters.",
+ this, SLOT( updateArrow3dGeometry() ) );
+
+ axes_length_property_ = new FloatProperty( "Axes Length", 0.3, "Length of each axis, in meters.",
+ this, SLOT( updateAxesGeometry() ) );
+
+ axes_radius_property_ = new FloatProperty( "Axes Radius", 0.01, "Radius of each axis, in meters.",
+ this, SLOT( updateAxesGeometry() ) );
+
+ shape_property_->addOption( "Arrow (Flat)", ShapeType::Arrow2d );
+ shape_property_->addOption( "Arrow (3D)", ShapeType::Arrow3d );
+ shape_property_->addOption( "Axes", ShapeType::Axes );
+ arrow_alpha_property_->setMin( 0 );
+ arrow_alpha_property_->setMax( 1 );
}
PoseArrayDisplay::~PoseArrayDisplay()
@@ -63,6 +122,9 @@ void PoseArrayDisplay::onInitialize()
manual_object_ = scene_manager_->createManualObject();
manual_object_->setDynamic( true );
scene_node_->attachObject( manual_object_ );
+ arrow_node_ = scene_node_->createChildSceneNode();
+ axes_node_ = scene_node_->createChildSceneNode();
+ updateShapeChoice();
}
bool validateFloats( const geometry_msgs::PoseArray& msg )
@@ -74,41 +136,58 @@ void PoseArrayDisplay::processMessage( const geometry_msgs::PoseArray::ConstPtr&
{
if( !validateFloats( *msg ))
{
- setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
+ setStatus( StatusProperty::Error, "Topic",
+ "Message contained invalid floating point values (nans or infs)" );
return;
}
- manual_object_->clear();
+ if( !setTransform( msg->header ) )
+ {
+ setStatus( StatusProperty::Error, "Topic", "Failed to look up transform" );
+ return;
+ }
+
+ poses_.resize( msg->poses.size() );
+ for (std::size_t i = 0; i < msg->poses.size(); ++i)
+ {
+ poses_[i].position = vectorRosToOgre( msg->poses[i].position );
+ poses_[i].orientation = quaternionRosToOgre( msg->poses[i].orientation );
+ }
+
+ updateDisplay();
+ context_->queueRender();
+}
+bool PoseArrayDisplay::setTransform( std_msgs::Header const & header )
+{
Ogre::Vector3 position;
Ogre::Quaternion orientation;
- if( !context_->getFrameManager()->getTransform( msg->header, position, orientation ))
+ if( !context_->getFrameManager()->getTransform( header, position, orientation ) )
{
- ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
+ ROS_ERROR( "Error transforming pose '%s' from frame '%s' to frame '%s'",
+ qPrintable( getName() ), header.frame_id.c_str(),
+ qPrintable( fixed_frame_ ) );
+ return false;
}
-
scene_node_->setPosition( position );
scene_node_->setOrientation( orientation );
+ return true;
+}
+void PoseArrayDisplay::updateArrows2d()
+{
manual_object_->clear();
- Ogre::ColourValue color = color_property_->getOgreColor();
- float length = length_property_->getFloat();
- size_t num_poses = msg->poses.size();
+ Ogre::ColourValue color = arrow_color_property_->getOgreColor();
+ color.a = arrow_alpha_property_->getFloat();
+ float length = arrow2d_length_property_->getFloat();
+ size_t num_poses = poses_.size();
manual_object_->estimateVertexCount( num_poses * 6 );
manual_object_->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST );
for( size_t i=0; i < num_poses; ++i )
{
- Ogre::Vector3 pos( msg->poses[i].position.x,
- msg->poses[i].position.y,
- msg->poses[i].position.z );
- Ogre::Quaternion orient( msg->poses[i].orientation.w,
- msg->poses[i].orientation.x,
- msg->poses[i].orientation.y,
- msg->poses[i].orientation.z );
- // orient here is not normalized, so the scale of the quaternion
- // will affect the scale of the arrow.
-
+ const Ogre::Vector3 & pos = poses_[i].position;
+ const Ogre::Quaternion & orient = poses_[i].orientation;
Ogre::Vector3 vertices[6];
vertices[0] = pos; // back of arrow
vertices[1] = pos + orient * Ogre::Vector3( length, 0, 0 ); // tip of arrow
@@ -124,8 +203,84 @@ void PoseArrayDisplay::processMessage( const geometry_msgs::PoseArray::ConstPtr&
}
}
manual_object_->end();
+}
- context_->queueRender();
+void PoseArrayDisplay::updateDisplay()
+{
+ int shape = shape_property_->getOptionInt();
+ switch (shape) {
+ case ShapeType::Arrow2d:
+ updateArrows2d();
+ arrows3d_.clear();
+ axes_.clear();
+ break;
+ case ShapeType::Arrow3d:
+ updateArrows3d();
+ manual_object_->clear();
+ axes_.clear();
+ break;
+ case ShapeType::Axes:
+ updateAxes();
+ manual_object_->clear();
+ arrows3d_.clear();
+ break;
+ }
+}
+
+void PoseArrayDisplay::updateArrows3d()
+{
+ while (arrows3d_.size() < poses_.size())
+ arrows3d_.push_back(makeArrow3d());
+ while (arrows3d_.size() > poses_.size())
+ arrows3d_.pop_back();
+
+ Ogre::Quaternion adjust_orientation( Ogre::Degree(-90), Ogre::Vector3::UNIT_Y );
+ for (std::size_t i = 0; i < poses_.size(); ++i)
+ {
+ arrows3d_[i].setPosition( poses_[i].position );
+ arrows3d_[i].setOrientation( poses_[i].orientation * adjust_orientation );
+ }
+}
+
+void PoseArrayDisplay::updateAxes()
+{
+ while (axes_.size() < poses_.size())
+ axes_.push_back(makeAxes());
+ while (axes_.size() > poses_.size())
+ axes_.pop_back();
+ for (std::size_t i = 0; i < poses_.size(); ++i)
+ {
+ axes_[i].setPosition( poses_[i].position );
+ axes_[i].setOrientation( poses_[i].orientation );
+ }
+}
+
+Arrow * PoseArrayDisplay::makeArrow3d()
+{
+ Ogre::ColourValue color = arrow_color_property_->getOgreColor();
+ color.a = arrow_alpha_property_->getFloat();
+
+ Arrow * arrow = new Arrow(
+ scene_manager_,
+ arrow_node_,
+ arrow3d_shaft_length_property_->getFloat(),
+ arrow3d_shaft_radius_property_->getFloat(),
+ arrow3d_head_length_property_->getFloat(),
+ arrow3d_head_radius_property_->getFloat()
+ );
+
+ arrow->setColor(color);
+ return arrow;
+}
+
+Axes * PoseArrayDisplay::makeAxes()
+{
+ return new Axes(
+ scene_manager_,
+ axes_node_,
+ axes_length_property_->getFloat(),
+ axes_radius_property_->getFloat()
+ );
}
void PoseArrayDisplay::reset()
@@ -135,6 +290,85 @@ void PoseArrayDisplay::reset()
{
manual_object_->clear();
}
+ arrows3d_.clear();
+ axes_.clear();
+}
+
+void PoseArrayDisplay::updateShapeChoice()
+{
+ int shape = shape_property_->getOptionInt();
+ bool use_arrow2d = shape == ShapeType::Arrow2d;
+ bool use_arrow3d = shape == ShapeType::Arrow3d;
+ bool use_arrow = use_arrow2d || use_arrow3d;
+ bool use_axes = shape == ShapeType::Axes;
+
+ arrow_color_property_->setHidden( !use_arrow );
+ arrow_alpha_property_->setHidden( !use_arrow );
+
+ arrow2d_length_property_->setHidden(!use_arrow2d);
+
+ arrow3d_shaft_length_property_->setHidden( !use_arrow3d );
+ arrow3d_shaft_radius_property_->setHidden( !use_arrow3d );
+ arrow3d_head_length_property_->setHidden( !use_arrow3d );
+ arrow3d_head_radius_property_->setHidden( !use_arrow3d );
+
+ axes_length_property_->setHidden( !use_axes );
+ axes_radius_property_->setHidden( !use_axes );
+
+ if (initialized())
+ updateDisplay();
+}
+
+void PoseArrayDisplay::updateArrowColor()
+{
+ int shape = shape_property_->getOptionInt();
+ Ogre::ColourValue color = arrow_color_property_->getOgreColor();
+ color.a = arrow_alpha_property_->getFloat();
+
+ if (shape == ShapeType::Arrow2d)
+ {
+ updateArrows2d();
+ }
+ else if (shape == ShapeType::Arrow3d)
+ {
+ for (std::size_t i = 0; i < arrows3d_.size(); ++i)
+ {
+ arrows3d_[i].setColor( color );
+ }
+ }
+ context_->queueRender();
+}
+
+void PoseArrayDisplay::updateArrow2dGeometry()
+{
+ updateArrows2d();
+ context_->queueRender();
+}
+
+void PoseArrayDisplay::updateArrow3dGeometry()
+{
+ for (std::size_t i = 0; i < poses_.size(); ++i)
+ {
+ arrows3d_[i].set(
+ arrow3d_shaft_length_property_->getFloat(),
+ arrow3d_shaft_radius_property_->getFloat(),
+ arrow3d_head_length_property_->getFloat(),
+ arrow3d_head_radius_property_->getFloat()
+ );
+ }
+ context_->queueRender();
+}
+
+void PoseArrayDisplay::updateAxesGeometry()
+{
+ for (std::size_t i = 0; i < poses_.size(); ++i)
+ {
+ axes_[i].set(
+ axes_length_property_->getFloat(),
+ axes_radius_property_->getFloat()
+ );
+ }
+ context_->queueRender();
}
} // namespace rviz
diff --git a/src/rviz/default_plugin/pose_array_display.h b/src/rviz/default_plugin/pose_array_display.h
index 64b9248..d73cce6 100644
--- a/src/rviz/default_plugin/pose_array_display.h
+++ b/src/rviz/default_plugin/pose_array_display.h
@@ -34,6 +34,8 @@
#include "rviz/message_filter_display.h"
+#include <boost/ptr_container/ptr_vector.hpp>
+
namespace Ogre
{
class ManualObject;
@@ -41,8 +43,12 @@ class ManualObject;
namespace rviz
{
+
+class EnumProperty;
class ColorProperty;
class FloatProperty;
+class Arrow;
+class Axes;
/** @brief Displays a geometry_msgs/PoseArray message as a bunch of line-drawn arrows. */
class PoseArrayDisplay: public MessageFilterDisplay<geometry_msgs::PoseArray>
@@ -58,10 +64,56 @@ protected:
virtual void processMessage( const geometry_msgs::PoseArray::ConstPtr& msg );
private:
+ bool setTransform(std_msgs::Header const & header);
+ void updateArrows2d();
+ void updateArrows3d();
+ void updateAxes();
+ void updateDisplay();
+ Axes * makeAxes();
+ Arrow * makeArrow3d();
+
+ struct OgrePose {
+ Ogre::Vector3 position;
+ Ogre::Quaternion orientation;
+ };
+
+ std::vector<OgrePose> poses_;
+ boost::ptr_vector<Arrow> arrows3d_;
+ boost::ptr_vector<Axes> axes_;
+
+ Ogre::SceneNode * arrow_node_;
+ Ogre::SceneNode * axes_node_;
Ogre::ManualObject* manual_object_;
- ColorProperty* color_property_;
- FloatProperty* length_property_;
+ EnumProperty* shape_property_;
+ ColorProperty* arrow_color_property_;
+ FloatProperty* arrow_alpha_property_;
+
+ FloatProperty* arrow2d_length_property_;
+
+ FloatProperty* arrow3d_head_radius_property_;
+ FloatProperty* arrow3d_head_length_property_;
+ FloatProperty* arrow3d_shaft_radius_property_;
+ FloatProperty* arrow3d_shaft_length_property_;
+
+ FloatProperty* axes_length_property_;
+ FloatProperty* axes_radius_property_;
+
+private Q_SLOTS:
+ /// Update the interface and visible shapes based on the selected shape type.
+ void updateShapeChoice();
+
+ /// Update the arrow color.
+ void updateArrowColor();
+
+ /// Update the flat arrow geometry.
+ void updateArrow2dGeometry();
+
+ /// Update the 3D arrow geometry.
+ void updateArrow3dGeometry();
+
+ /// Update the axes geometry.
+ void updateAxesGeometry();
};
} // namespace rviz
diff --git a/src/rviz/default_plugin/view_controllers/orbit_view_controller.cpp b/src/rviz/default_plugin/view_controllers/orbit_view_controller.cpp
index 121baa3..006a3f3 100644
--- a/src/rviz/default_plugin/view_controllers/orbit_view_controller.cpp
+++ b/src/rviz/default_plugin/view_controllers/orbit_view_controller.cpp
@@ -39,6 +39,7 @@
#include "rviz/display_context.h"
#include "rviz/geometry.h"
#include "rviz/ogre_helpers/shape.h"
+#include "rviz/properties/bool_property.h"
#include "rviz/properties/float_property.h"
#include "rviz/properties/vector_property.h"
#include "rviz/uniform_string_stream.h"
@@ -51,6 +52,8 @@
static const float PITCH_START = Ogre::Math::HALF_PI / 2.0;
static const float YAW_START = Ogre::Math::HALF_PI * 0.5;
static const float DISTANCE_START = 10;
+static const float FOCAL_SHAPE_SIZE_START = 0.05;
+static const bool FOCAL_SHAPE_FIXED_SIZE = true;
namespace rviz
{
@@ -61,6 +64,11 @@ OrbitViewController::OrbitViewController()
distance_property_ = new FloatProperty( "Distance", DISTANCE_START, "Distance from the focal point.", this );
distance_property_->setMin( 0.01 );
+ focal_shape_size_property_ = new FloatProperty( "Focal Shape Size", FOCAL_SHAPE_SIZE_START, "Focal shape size.", this );
+ focal_shape_size_property_->setMin( 0.001 );
+
+ focal_shape_fixed_size_property_ = new BoolProperty ( "Focal Shape Fixed Size", FOCAL_SHAPE_FIXED_SIZE, "Focal shape size.", this );
+
yaw_property_ = new FloatProperty( "Yaw", YAW_START, "Rotation of the camera around the Z (up) axis.", this );
pitch_property_ = new FloatProperty( "Pitch", PITCH_START, "How much the camera is tipped downward.", this );
@@ -77,7 +85,7 @@ void OrbitViewController::onInitialize()
camera_->setProjectionType( Ogre::PT_PERSPECTIVE );
focal_shape_ = new Shape(Shape::Sphere, context_->getSceneManager(), target_scene_node_);
- focal_shape_->setScale(Ogre::Vector3(0.05f, 0.05f, 0.01f));
+ updateFocalShapeSize();
focal_shape_->setColor(1.0f, 1.0f, 0.0f, 0.5f);
focal_shape_->getRootNode()->setVisible(false);
}
@@ -93,6 +101,9 @@ void OrbitViewController::reset()
yaw_property_->setFloat( YAW_START );
pitch_property_->setFloat( PITCH_START );
distance_property_->setFloat( DISTANCE_START );
+ focal_shape_size_property_->setFloat( FOCAL_SHAPE_SIZE_START );
+ focal_shape_fixed_size_property_->setBool( false );
+ updateFocalShapeSize();
focal_point_property_->setVector( Ogre::Vector3::ZERO );
}
@@ -108,6 +119,7 @@ void OrbitViewController::handleMouseEvent(ViewportMouseEvent& event)
}
float distance = distance_property_->getFloat();
+ updateFocalShapeSize();
int32_t diff_x = 0;
int32_t diff_y = 0;
@@ -209,12 +221,14 @@ void OrbitViewController::mimic( ViewController* source_view )
{
// If I'm initializing from another instance of this same class, get the distance exactly.
distance_property_->setFloat( source_view->subProp( "Distance" )->getValue().toFloat() );
+ updateFocalShapeSize();
}
else
{
// Determine the distance from here to the reference frame, and use
// that as the distance our focal point should be at.
distance_property_->setFloat( position.length() );
+ updateFocalShapeSize();
}
Ogre::Vector3 direction = orientation * (Ogre::Vector3::NEGATIVE_UNIT_Z * distance_property_->getFloat() );
@@ -234,7 +248,7 @@ void OrbitViewController::lookAt( const Ogre::Vector3& point )
Ogre::Vector3 camera_position = camera_->getPosition();
focal_point_property_->setVector( target_scene_node_->getOrientation().Inverse() * (point - target_scene_node_->getPosition()) );
distance_property_->setFloat( focal_point_property_->getVector().distance( camera_position ));
-
+ updateFocalShapeSize();
calculatePitchYawFromPosition(camera_position);
}
@@ -255,6 +269,7 @@ void OrbitViewController::updateCamera()
float y = distance * sin( yaw ) * cos( pitch ) + focal_point.y;
float z = distance * sin( pitch ) + focal_point.z;
+
Ogre::Vector3 pos( x, y, z );
camera_->setPosition(pos);
@@ -281,9 +296,22 @@ void OrbitViewController::calculatePitchYawFromPosition( const Ogre::Vector3& po
yaw_property_->setFloat( atan2( diff.y, diff.x ));
}
+void OrbitViewController::updateFocalShapeSize()
+{
+ const double fshape_size( focal_shape_size_property_->getFloat() );
+ double distance_property( distance_property_->getFloat() );
+ if( focal_shape_fixed_size_property_->getBool() )
+ distance_property = 1;
+
+ focal_shape_->setScale( Ogre::Vector3( fshape_size * distance_property,
+ fshape_size * distance_property,
+ fshape_size * distance_property / 5.0 ) );
+}
+
void OrbitViewController::zoom( float amount )
{
distance_property_->add( -amount );
+ updateFocalShapeSize();
}
void OrbitViewController::move( float x, float y, float z )
diff --git a/src/rviz/default_plugin/view_controllers/orbit_view_controller.h b/src/rviz/default_plugin/view_controllers/orbit_view_controller.h
index 62c4d7b..66a0655 100644
--- a/src/rviz/default_plugin/view_controllers/orbit_view_controller.h
+++ b/src/rviz/default_plugin/view_controllers/orbit_view_controller.h
@@ -38,6 +38,7 @@
namespace rviz
{
+class BoolProperty;
class FloatProperty;
class Shape;
class SceneNode;
@@ -101,12 +102,19 @@ protected:
*/
void calculatePitchYawFromPosition( const Ogre::Vector3& position );
+ /**
+ * \brief Calculates the focal shape size and update it's geometry
+ */
+ void updateFocalShapeSize();
+
virtual void updateCamera();
FloatProperty* yaw_property_; ///< The camera's yaw (rotation around the y-axis), in radians
FloatProperty* pitch_property_; ///< The camera's pitch (rotation around the x-axis), in radians
FloatProperty* distance_property_; ///< The camera's distance from the focal point
VectorProperty* focal_point_property_; ///< The point around which the camera "orbits".
+ BoolProperty* focal_shape_fixed_size_property_; ///< Whether the focal shape size is fixed or not
+ FloatProperty* focal_shape_size_property_; ///< The focal shape size
Shape* focal_shape_;
bool dragging_;
diff --git a/src/rviz/display.h b/src/rviz/display.h
index 37807b4..c17ae5c 100644
--- a/src/rviz/display.h
+++ b/src/rviz/display.h
@@ -113,7 +113,11 @@ public:
* @param topic The published topic to be visualized.
* @param datatype The datatype of the topic.
*/
- virtual void setTopic( const QString &topic, const QString &datatype ) { }
+ virtual void setTopic( const QString &topic, const QString &datatype )
+ {
+ (void) topic;
+ (void) datatype;
+ }
/** @brief Return true if this Display is enabled, false if not. */
bool isEnabled() const;
@@ -124,7 +128,11 @@ public:
/** @brief Called periodically by the visualization manager.
* @param wall_dt Wall-clock time, in seconds, since the last time the update list was run through.
* @param ros_dt ROS time, in seconds, since the last time the update list was run through. */
- virtual void update( float wall_dt, float ros_dt ) {}
+ virtual void update( float wall_dt, float ros_dt )
+ {
+ (void) wall_dt;
+ (void) ros_dt;
+ }
/** @brief Called to tell the display to clear its state */
virtual void reset();
diff --git a/src/rviz/display_group.cpp b/src/rviz/display_group.cpp
index 7907099..0c46181 100644
--- a/src/rviz/display_group.cpp
+++ b/src/rviz/display_group.cpp
@@ -164,10 +164,11 @@ void DisplayGroup::removeAllDisplays()
}
for( int i = displays_.size() - 1; i >= 0; i-- )
{
-// printf(" displaygroup2 displays_.takeAt( %d )\n", i );
Display* child = displays_.takeAt( i );
Q_EMIT displayRemoved( child );
child->setParent( NULL ); // prevent child destructor from calling getParent()->takeChild().
+ child->setModel( NULL );
+ child_indexes_valid_ = false;
delete child;
}
if( model_ )
@@ -189,7 +190,6 @@ Display* DisplayGroup::takeDisplay( Display* child )
{
model_->beginRemove( this, Display::numChildren() + i, 1 );
}
-// printf(" displaygroup3 displays_.takeAt( %d )\n", i );
result = displays_.takeAt( i );
Q_EMIT displayRemoved( result );
result->setParent( NULL );
diff --git a/src/rviz/displays_panel.cpp b/src/rviz/displays_panel.cpp
index 741a289..9ca2945 100644
--- a/src/rviz/displays_panel.cpp
+++ b/src/rviz/displays_panel.cpp
@@ -131,6 +131,7 @@ void DisplaysPanel::onNewDisplay()
}
vis_manager_->startUpdate();
activateWindow(); // Force keyboard focus back on main window.
+ delete dialog;
}
void DisplaysPanel::onDuplicateDisplay()
diff --git a/src/rviz/ogre_helpers/grid.cpp b/src/rviz/ogre_helpers/grid.cpp
index 885fe3f..a052484 100644
--- a/src/rviz/ogre_helpers/grid.cpp
+++ b/src/rviz/ogre_helpers/grid.cpp
@@ -240,7 +240,7 @@ void Grid::create()
void Grid::setUserData( const Ogre::Any& data )
{
- manual_object_->setUserAny( data );
+ manual_object_->getUserObjectBindings().setUserAny( data );
}
} // namespace rviz
diff --git a/src/rviz/ogre_helpers/line.cpp b/src/rviz/ogre_helpers/line.cpp
index ba913c8..bd213cd 100644
--- a/src/rviz/ogre_helpers/line.cpp
+++ b/src/rviz/ogre_helpers/line.cpp
@@ -145,7 +145,7 @@ const Ogre::Quaternion& Line::getOrientation()
void Line::setUserData( const Ogre::Any& data )
{
- manual_object_->setUserAny( data );
+ manual_object_->getUserObjectBindings().setUserAny( data );
}
diff --git a/src/rviz/ogre_helpers/shape.cpp b/src/rviz/ogre_helpers/shape.cpp
index dd73056..9ea6846 100644
--- a/src/rviz/ogre_helpers/shape.cpp
+++ b/src/rviz/ogre_helpers/shape.cpp
@@ -177,7 +177,7 @@ const Ogre::Quaternion& Shape::getOrientation()
void Shape::setUserData( const Ogre::Any& data )
{
if (entity_)
- entity_->setUserAny( data );
+ entity_->getUserObjectBindings().setUserAny( data );
else
ROS_ERROR("Shape not yet fully constructed. Cannot set user data. Did you add triangles to the mesh already?");
}
diff --git a/src/rviz/properties/float_edit.cpp b/src/rviz/properties/float_edit.cpp
index 18ed2a5..74bc48b 100644
--- a/src/rviz/properties/float_edit.cpp
+++ b/src/rviz/properties/float_edit.cpp
@@ -28,6 +28,7 @@
*/
#include <QDoubleValidator>
+#include <QLocale>
#include "rviz/properties/float_edit.h"
@@ -46,12 +47,13 @@ void FloatEdit::setValue( float new_value )
{
if( value_ != new_value )
{
+ QLocale locale;
value_ = new_value;
bool ok = true;
- float existing_text_value = text().toFloat( &ok );
+ float existing_text_value = locale.toFloat(text(), &ok );
if( !ok || existing_text_value != new_value )
{
- setText( QString::number( (double) value_ ));
+ setText( locale.toString((double) value_ ));
}
}
}
@@ -61,7 +63,7 @@ void FloatEdit::updateValue()
if( hasAcceptableInput() )
{
bool ok = true;
- float new_value = text().toFloat( &ok );
+ float new_value = QLocale().toFloat(text(), &ok );
if( ok )
{
setValue( new_value );
diff --git a/src/rviz/properties/property.h b/src/rviz/properties/property.h
index e77177a..5643031 100644
--- a/src/rviz/properties/property.h
+++ b/src/rviz/properties/property.h
@@ -273,8 +273,12 @@ public:
* this function to do the painting and return true.
*
* If this function returns false, a QStyledItemDelegate will do the painting. */
- virtual bool paint( QPainter* painter,
- const QStyleOptionViewItem& option ) const { return false; }
+ virtual bool paint( QPainter* painter, const QStyleOptionViewItem& option ) const
+ {
+ (void) painter;
+ (void) option;
+ return false;
+ }
/** @brief Create an editor widget to edit the value of this property.
diff --git a/src/rviz/render_panel.cpp b/src/rviz/render_panel.cpp
index 23eb599..5594258 100644
--- a/src/rviz/render_panel.cpp
+++ b/src/rviz/render_panel.cpp
@@ -66,6 +66,10 @@ RenderPanel::~RenderPanel()
{
scene_manager_->destroyCamera( default_camera_ );
}
+ if( scene_manager_ )
+ {
+ scene_manager_->removeListener( this );
+ }
}
void RenderPanel::initialize(Ogre::SceneManager* scene_manager, DisplayContext* context)
diff --git a/src/rviz/robot/robot.cpp b/src/rviz/robot/robot.cpp
index a65c129..c1a87f0 100644
--- a/src/rviz/robot/robot.cpp
+++ b/src/rviz/robot/robot.cpp
@@ -236,7 +236,7 @@ void Robot::clear()
RobotLink* Robot::LinkFactory::createLink(
Robot* robot,
- const boost::shared_ptr<const urdf::Link>& link,
+ const urdf::LinkConstSharedPtr& link,
const std::string& parent_joint_name,
bool visual,
bool collision)
@@ -246,7 +246,7 @@ RobotLink* Robot::LinkFactory::createLink(
RobotJoint* Robot::LinkFactory::createJoint(
Robot* robot,
- const boost::shared_ptr<const urdf::Joint>& joint)
+ const urdf::JointConstSharedPtr& joint)
{
return new RobotJoint(robot, joint);
}
@@ -265,12 +265,12 @@ void Robot::load( const urdf::ModelInterface &urdf, bool visual, bool collision
// Create properties for each link.
// Properties are not added to display until changedLinkTreeStyle() is called (below).
{
- typedef std::map<std::string, boost::shared_ptr<urdf::Link> > M_NameToUrdfLink;
+ typedef std::map<std::string, urdf::LinkSharedPtr > M_NameToUrdfLink;
M_NameToUrdfLink::const_iterator link_it = urdf.links_.begin();
M_NameToUrdfLink::const_iterator link_end = urdf.links_.end();
for( ; link_it != link_end; ++link_it )
{
- const boost::shared_ptr<const urdf::Link>& urdf_link = link_it->second;
+ const urdf::LinkConstSharedPtr& urdf_link = link_it->second;
std::string parent_joint_name;
if (urdf_link != urdf.getRoot() && urdf_link->parent_joint)
@@ -298,12 +298,12 @@ void Robot::load( const urdf::ModelInterface &urdf, bool visual, bool collision
// Create properties for each joint.
// Properties are not added to display until changedLinkTreeStyle() is called (below).
{
- typedef std::map<std::string, boost::shared_ptr<urdf::Joint> > M_NameToUrdfJoint;
+ typedef std::map<std::string, urdf::JointSharedPtr > M_NameToUrdfJoint;
M_NameToUrdfJoint::const_iterator joint_it = urdf.joints_.begin();
M_NameToUrdfJoint::const_iterator joint_end = urdf.joints_.end();
for( ; joint_it != joint_end; ++joint_it )
{
- const boost::shared_ptr<const urdf::Joint>& urdf_joint = joint_it->second;
+ const urdf::JointConstSharedPtr& urdf_joint = joint_it->second;
RobotJoint* joint = link_factory_->createJoint( this, urdf_joint );
joints_[urdf_joint->name] = joint;
@@ -682,7 +682,14 @@ void Robot::calculateJointCheckboxes()
// check root link
RobotLink *link = root_link_;
- if (link && link->hasGeometry())
+
+ if(!link)
+ {
+ setEnableAllLinksCheckbox(QVariant());
+ return;
+ }
+
+ if (link->hasGeometry())
{
bool checked = link->getLinkProperty()->getValue().toBool();
links_with_geom_checked += checked ? 1 : 0;
diff --git a/src/rviz/robot/robot.h b/src/rviz/robot/robot.h
index d529177..ff0afa7 100644
--- a/src/rviz/robot/robot.h
+++ b/src/rviz/robot/robot.h
@@ -39,6 +39,9 @@
#include <OgreQuaternion.h>
#include <OgreAny.h>
+#include <urdf_model/types.h>
+#include <urdf_world/types.h>
+
namespace Ogre
{
class SceneManager;
@@ -62,13 +65,6 @@ namespace tf
class TransformListener;
}
-namespace urdf
-{
-class ModelInterface;
-class Link;
-class Joint;
-}
-
namespace rviz
{
@@ -173,12 +169,12 @@ public:
{
public:
virtual RobotLink* createLink( Robot* robot,
- const boost::shared_ptr<const urdf::Link>& link,
+ const urdf::LinkConstSharedPtr& link,
const std::string& parent_joint_name,
bool visual,
bool collision);
virtual RobotJoint* createJoint( Robot* robot,
- const boost::shared_ptr<const urdf::Joint>& joint);
+ const urdf::JointConstSharedPtr& joint);
};
/** Call this before load() to subclass the RobotLink or RobotJoint class used in the link property.
diff --git a/src/rviz/robot/robot_joint.cpp b/src/rviz/robot/robot_joint.cpp
index 6538fd0..eade172 100644
--- a/src/rviz/robot/robot_joint.cpp
+++ b/src/rviz/robot/robot_joint.cpp
@@ -38,15 +38,13 @@
#include "rviz/ogre_helpers/axes.h"
#include "rviz/load_resource.h"
-#include <urdf_model/model.h>
-#include <urdf_model/link.h>
-#include <urdf_model/joint.h>
+#include <urdf_world/types.h>
namespace rviz
{
-RobotJoint::RobotJoint( Robot* robot, const boost::shared_ptr<const urdf::Joint>& joint )
+RobotJoint::RobotJoint( Robot* robot, const urdf::JointConstSharedPtr& joint )
: robot_( robot )
, name_( joint->name )
, child_link_name_( joint->child_link_name )
diff --git a/src/rviz/robot/robot_joint.h b/src/rviz/robot/robot_joint.h
index ba8a832..f9242a0 100644
--- a/src/rviz/robot/robot_joint.h
+++ b/src/rviz/robot/robot_joint.h
@@ -42,6 +42,10 @@
#include <OgreMaterial.h>
#endif
+#include <urdf/model.h>
+#include <urdf_model/pose.h>
+#include <urdf_world/types.h>
+
#include "rviz/ogre_helpers/object.h"
#include "rviz/selection/forwards.h"
@@ -57,15 +61,6 @@ class Any;
class RibbonTrail;
}
-namespace urdf
-{
-class ModelInterface;
-class Link;
-class Joint;
-class Geometry;
-class Pose;
-}
-
namespace rviz
{
class Shape;
@@ -89,7 +84,7 @@ class RobotJoint: public QObject
{
Q_OBJECT
public:
- RobotJoint( Robot* robot, const boost::shared_ptr<const urdf::Joint>& joint );
+ RobotJoint( Robot* robot, const urdf::JointConstSharedPtr& joint );
virtual ~RobotJoint();
diff --git a/src/rviz/robot/robot_link.cpp b/src/rviz/robot/robot_link.cpp
index e313207..4d9a4ca 100644
--- a/src/rviz/robot/robot_link.cpp
+++ b/src/rviz/robot/robot_link.cpp
@@ -43,7 +43,6 @@
#include <ros/console.h>
#include <resource_retriever/retriever.h>
-
#include <urdf_model/model.h>
#include <urdf_model/link.h>
@@ -155,7 +154,7 @@ void RobotLinkSelectionHandler::postRenderPass(uint32_t pass)
RobotLink::RobotLink( Robot* robot,
- const urdf::LinkConstPtr& link,
+ const urdf::LinkConstSharedPtr& link,
const std::string& parent_joint_name,
bool visual,
bool collision)
@@ -262,8 +261,8 @@ RobotLink::RobotLink( Robot* robot,
desc << " child joint: ";
}
- std::vector<boost::shared_ptr<urdf::Joint> >::const_iterator child_it = link->child_joints.begin();
- std::vector<boost::shared_ptr<urdf::Joint> >::const_iterator child_end = link->child_joints.end();
+ std::vector<urdf::JointSharedPtr >::const_iterator child_it = link->child_joints.begin();
+ std::vector<urdf::JointSharedPtr >::const_iterator child_end = link->child_joints.end();
for ( ; child_it != child_end ; ++child_it )
{
urdf::Joint *child_joint = child_it->get();
@@ -442,7 +441,7 @@ void RobotLink::updateVisibility()
}
}
-Ogre::MaterialPtr RobotLink::getMaterialForLink( const urdf::LinkConstPtr& link)
+Ogre::MaterialPtr RobotLink::getMaterialForLink( const urdf::LinkConstSharedPtr& link)
{
if (!link->visual || !link->visual->material)
{
@@ -510,7 +509,7 @@ Ogre::MaterialPtr RobotLink::getMaterialForLink( const urdf::LinkConstPtr& link)
return mat;
}
-void RobotLink::createEntityForGeometryElement(const urdf::LinkConstPtr& link, const urdf::Geometry& geom, const urdf::Pose& origin, Ogre::SceneNode* scene_node, Ogre::Entity*& entity)
+void RobotLink::createEntityForGeometryElement(const urdf::LinkConstSharedPtr& link, const urdf::Geometry& geom, const urdf::Pose& origin, Ogre::SceneNode* scene_node, Ogre::Entity*& entity)
{
entity = NULL; // default in case nothing works.
Ogre::SceneNode* offset_node = scene_node->createChildSceneNode();
@@ -647,19 +646,19 @@ void RobotLink::createEntityForGeometryElement(const urdf::LinkConstPtr& link, c
}
}
-void RobotLink::createCollision(const urdf::LinkConstPtr& link)
+void RobotLink::createCollision(const urdf::LinkConstSharedPtr& link)
{
bool valid_collision_found = false;
#if URDF_MAJOR_VERSION == 0 && URDF_MINOR_VERSION == 2
- std::map<std::string, boost::shared_ptr<std::vector<boost::shared_ptr<urdf::Collision> > > >::const_iterator mi;
+ std::map<std::string, boost::shared_ptr<std::vector<urdf::CollisionSharedPtr > > >::const_iterator mi;
for( mi = link->collision_groups.begin(); mi != link->collision_groups.end(); mi++ )
{
if( mi->second )
{
- std::vector<boost::shared_ptr<urdf::Collision> >::const_iterator vi;
+ std::vector<urdf::CollisionSharedPtr >::const_iterator vi;
for( vi = mi->second->begin(); vi != mi->second->end(); vi++ )
{
- boost::shared_ptr<urdf::Collision> collision = *vi;
+ urdf::CollisionSharedPtr collision = *vi;
if( collision && collision->geometry )
{
Ogre::Entity* collision_mesh = NULL;
@@ -674,10 +673,10 @@ void RobotLink::createCollision(const urdf::LinkConstPtr& link)
}
}
#else
- std::vector<boost::shared_ptr<urdf::Collision> >::const_iterator vi;
+ std::vector<urdf::CollisionSharedPtr >::const_iterator vi;
for( vi = link->collision_array.begin(); vi != link->collision_array.end(); vi++ )
{
- boost::shared_ptr<urdf::Collision> collision = *vi;
+ urdf::CollisionSharedPtr collision = *vi;
if( collision && collision->geometry )
{
Ogre::Entity* collision_mesh = NULL;
@@ -704,19 +703,19 @@ void RobotLink::createCollision(const urdf::LinkConstPtr& link)
collision_node_->setVisible( getEnabled() );
}
-void RobotLink::createVisual(const urdf::LinkConstPtr& link )
+void RobotLink::createVisual(const urdf::LinkConstSharedPtr& link )
{
bool valid_visual_found = false;
#if URDF_MAJOR_VERSION == 0 && URDF_MINOR_VERSION == 2
- std::map<std::string, boost::shared_ptr<std::vector<boost::shared_ptr<urdf::Visual> > > >::const_iterator mi;
+ std::map<std::string, boost::shared_ptr<std::vector<urdf::VisualSharedPtr > > >::const_iterator mi;
for( mi = link->visual_groups.begin(); mi != link->visual_groups.end(); mi++ )
{
if( mi->second )
{
- std::vector<boost::shared_ptr<urdf::Visual> >::const_iterator vi;
+ std::vector<urdf::VisualSharedPtr >::const_iterator vi;
for( vi = mi->second->begin(); vi != mi->second->end(); vi++ )
{
- boost::shared_ptr<urdf::Visual> visual = *vi;
+ urdf::VisualSharedPtr visual = *vi;
if( visual && visual->geometry )
{
Ogre::Entity* visual_mesh = NULL;
@@ -731,10 +730,10 @@ void RobotLink::createVisual(const urdf::LinkConstPtr& link )
}
}
#else
- std::vector<boost::shared_ptr<urdf::Visual> >::const_iterator vi;
+ std::vector<urdf::VisualSharedPtr >::const_iterator vi;
for( vi = link->visual_array.begin(); vi != link->visual_array.end(); vi++ )
{
- boost::shared_ptr<urdf::Visual> visual = *vi;
+ urdf::VisualSharedPtr visual = *vi;
if( visual && visual->geometry )
{
Ogre::Entity* visual_mesh = NULL;
diff --git a/src/rviz/robot/robot_link.h b/src/rviz/robot/robot_link.h
index 31e8e6f..d0014fb 100644
--- a/src/rviz/robot/robot_link.h
+++ b/src/rviz/robot/robot_link.h
@@ -43,6 +43,9 @@
#include <OgreSharedPtr.h>
#endif
+#include <urdf_model/types.h>
+#include <urdf_model/pose.h>
+
#include "rviz/ogre_helpers/object.h"
#include "rviz/selection/forwards.h"
@@ -58,16 +61,6 @@ class Any;
class RibbonTrail;
}
-namespace urdf
-{
-class ModelInterface;
-class Link;
-typedef boost::shared_ptr<const Link> LinkConstPtr;
-class Geometry;
-typedef boost::shared_ptr<const Geometry> GeometryConstPtr;
-class Pose;
-}
-
namespace rviz
{
class Shape;
@@ -93,7 +86,7 @@ class RobotLink: public QObject
Q_OBJECT
public:
RobotLink( Robot* robot,
- const urdf::LinkConstPtr& link,
+ const urdf::LinkConstSharedPtr& link,
const std::string& parent_joint_name,
bool visual,
bool collision);
@@ -162,12 +155,12 @@ private Q_SLOTS:
private:
void setRenderQueueGroup( Ogre::uint8 group );
bool getEnabled() const;
- void createEntityForGeometryElement( const urdf::LinkConstPtr& link, const urdf::Geometry& geom, const urdf::Pose& origin, Ogre::SceneNode* scene_node, Ogre::Entity*& entity );
+ void createEntityForGeometryElement( const urdf::LinkConstSharedPtr& link, const urdf::Geometry& geom, const urdf::Pose& origin, Ogre::SceneNode* scene_node, Ogre::Entity*& entity );
- void createVisual( const urdf::LinkConstPtr& link);
- void createCollision( const urdf::LinkConstPtr& link);
+ void createVisual( const urdf::LinkConstSharedPtr& link);
+ void createCollision( const urdf::LinkConstSharedPtr& link);
void createSelection();
- Ogre::MaterialPtr getMaterialForLink( const urdf::LinkConstPtr& link );
+ Ogre::MaterialPtr getMaterialForLink( const urdf::LinkConstSharedPtr& link );
protected:
diff --git a/src/rviz/tool.h b/src/rviz/tool.h
index a77d8bc..66db59a 100644
--- a/src/rviz/tool.h
+++ b/src/rviz/tool.h
@@ -81,7 +81,11 @@ public:
virtual void activate() = 0;
virtual void deactivate() = 0;
- virtual void update(float wall_dt, float ros_dt) {}
+ virtual void update(float wall_dt, float ros_dt)
+ {
+ (void) wall_dt;
+ (void) ros_dt;
+ }
enum {
Render = 1,
@@ -90,12 +94,21 @@ public:
/** Process a mouse event. This is the central function of all the
* tools, as it defines how the mouse is used. */
- virtual int processMouseEvent( ViewportMouseEvent& event ) { return 0; }
+ virtual int processMouseEvent( ViewportMouseEvent& event )
+ {
+ (void) event;
+ return 0;
+ }
/** Process a key event. Override if your tool should handle any
other keypresses than the tool shortcuts, which are handled
separately. */
- virtual int processKeyEvent( QKeyEvent* event, RenderPanel* panel ) { return 0; }
+ virtual int processKeyEvent( QKeyEvent* event, RenderPanel* panel )
+ {
+ (void) event;
+ (void) panel;
+ return 0;
+ }
QString getName() const { return name_; }
@@ -147,6 +160,9 @@ public:
const QCursor& getCursor() { return cursor_; }
void setStatus( const QString & message );
+
+Q_SIGNALS:
+ void close();
protected:
/** Override onInitialize to do any setup needed after the
diff --git a/src/rviz/tool_manager.cpp b/src/rviz/tool_manager.cpp
index 44b42b5..1ec5b19 100644
--- a/src/rviz/tool_manager.cpp
+++ b/src/rviz/tool_manager.cpp
@@ -220,6 +220,11 @@ void ToolManager::updatePropertyVisibility( Property* container )
}
}
+void rviz::ToolManager::closeTool()
+{
+ setCurrentTool( getDefaultTool() );
+}
+
Tool* ToolManager::addTool( const QString& class_id )
{
QString error;
@@ -260,6 +265,9 @@ Tool* ToolManager::addTool( const QString& class_id )
setDefaultTool( tool );
setCurrentTool( tool );
}
+
+ QObject::connect(tool, SIGNAL(close()),
+ this, SLOT(closeTool()));
Q_EMIT configChanged();
diff --git a/src/rviz/tool_manager.h b/src/rviz/tool_manager.h
index 261fb17..60b2cf7 100644
--- a/src/rviz/tool_manager.h
+++ b/src/rviz/tool_manager.h
@@ -137,6 +137,9 @@ private Q_SLOTS:
/** @brief If @a property has children, it is added to the tool
* property tree, and if it does not, it is removed. */
void updatePropertyVisibility( Property* property );
+
+ /** @brief Deactivates the current tool and sets the default tool. */
+ void closeTool();
private:
diff --git a/src/rviz/view_controller.h b/src/rviz/view_controller.h
index 06333f2..2bcb325 100644
--- a/src/rviz/view_controller.h
+++ b/src/rviz/view_controller.h
@@ -86,9 +86,16 @@ public:
/** @brief Called at 30Hz by ViewManager::update() while this view
* is active. Override with code that needs to run repeatedly. */
- virtual void update(float dt, float ros_dt) {}
+ virtual void update(float dt, float ros_dt)
+ {
+ (void) dt;
+ (void) ros_dt;
+ }
- virtual void handleMouseEvent(ViewportMouseEvent& evt) {}
+ virtual void handleMouseEvent(ViewportMouseEvent& evt)
+ {
+ (void) evt;
+ }
/** @brief Called by MoveTool and InteractionTool when keyboard events are passed to them.
*
@@ -102,7 +109,10 @@ public:
/** @brief This should be implemented in each subclass to aim the
* camera at the given point in space (relative to the fixed
* frame). */
- virtual void lookAt( const Ogre::Vector3& point ) {}
+ virtual void lookAt( const Ogre::Vector3& point )
+ {
+ (void) point;
+ }
/** Reset the view controller to some sane initial state, like
* looking at 0,0,0 from a few meters away. */
@@ -115,7 +125,10 @@ public:
* @a source_view must return a valid @c Ogre::Camera* from getCamera().
*
* This base class implementation does nothing. */
- virtual void mimic( ViewController* source_view ) {}
+ virtual void mimic( ViewController* source_view )
+ {
+ (void) source_view;
+ }
/** @brief Called by ViewManager when this ViewController is being made current.
* @param previous_view is the previous "current" view, and will not be NULL.
@@ -125,7 +138,10 @@ public:
* viewpoint.
*
* This base class implementation does nothing. */
- virtual void transitionFrom( ViewController* previous_view ) {}
+ virtual void transitionFrom( ViewController* previous_view )
+ {
+ (void) previous_view;
+ }
/** @brief Subclasses should call this whenever a change is made which would change the results of toString(). */
void emitConfigChanged();
diff --git a/src/rviz/visualization_manager.cpp b/src/rviz/visualization_manager.cpp
index 822cdc5..84f857f 100644
--- a/src/rviz/visualization_manager.cpp
+++ b/src/rviz/visualization_manager.cpp
@@ -125,8 +125,10 @@ VisualizationManager::VisualizationManager( RenderPanel* render_panel, WindowMan
, frame_count_(0)
, window_manager_(wm)
, private_( new VisualizationManagerPrivate )
-, default_visibility_bit_( visibility_bit_allocator_.allocBit() )
{
+ // visibility_bit_allocator_ is listed after default_visibility_bit_ (and thus initialized later be default):
+ default_visibility_bit_ = visibility_bit_allocator_.allocBit();
+
frame_manager_ = new FrameManager(tf);
render_panel->setAutoRender(false);
diff --git a/src/rviz/visualizer_app.cpp b/src/rviz/visualizer_app.cpp
index 62523ad..37cdecd 100644
--- a/src/rviz/visualizer_app.cpp
+++ b/src/rviz/visualizer_app.cpp
@@ -274,7 +274,7 @@ bool VisualizerApp::init( int argc, char** argv )
frame_->setHelpPath( QString::fromStdString( help_path ));
}
frame_->setShowChooseNewMaster( in_mc_wrapper );
- if( splash_path != "" )
+ if( vm.count("splash-screen") )
{
frame_->setSplashPath( QString::fromStdString( splash_path ));
}
diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt
index fa4581a..6f8c6dc 100644
--- a/src/test/CMakeLists.txt
+++ b/src/test/CMakeLists.txt
@@ -3,16 +3,25 @@ find_package(rostest REQUIRED)
# This is a test utility which publishes images of different types.
add_executable(send_images EXCLUDE_FROM_ALL send_images.cpp)
+if(NOT WIN32)
+ set_target_properties(send_images PROPERTIES COMPILE_FLAGS "-std=c++11")
+endif()
target_link_libraries(send_images ${catkin_LIBRARIES})
add_dependencies(tests send_images)
# This is a test utility which can publish different kinds of markers.
add_executable(marker_test EXCLUDE_FROM_ALL marker_test.cpp)
+if(NOT WIN32)
+ set_target_properties(marker_test PROPERTIES COMPILE_FLAGS "-std=c++11")
+endif()
target_link_libraries(marker_test ${catkin_LIBRARIES})
add_dependencies(tests marker_test)
# This is a test utility which can publish different kinds of mesh markers.
add_executable(mesh_marker_test EXCLUDE_FROM_ALL mesh_marker_test.cpp)
+if(NOT WIN32)
+ set_target_properties(mesh_marker_test PROPERTIES COMPILE_FLAGS "-std=c++11")
+endif()
target_link_libraries(mesh_marker_test ${catkin_LIBRARIES})
add_dependencies(tests mesh_marker_test)
@@ -23,9 +32,6 @@ catkin_add_gtest(property_test
${MOC_MOCK_PROPERTY_CHANGE_RECEIVER}
)
target_link_libraries(property_test rviz ${catkin_LIBRARIES} ${QT_LIBRARIES})
-if (UseQt5)
- target_link_libraries(property_test Qt5::Widgets)
-endif()
# TODO(wjwwood): Fix this test, it used to use a set of Mock classes, but
# has since undergone a lot of changes and it no longer works.
@@ -50,61 +56,82 @@ catkin_add_gtest(uniform_string_stream_test
# This is an example node which serves an rviz logo as an interactive marker.
add_executable(rviz_logo_marker EXCLUDE_FROM_ALL rviz_logo_marker.cpp)
+if(NOT WIN32)
+ set_target_properties(rviz_logo_marker PROPERTIES COMPILE_FLAGS "-std=c++11")
+endif()
target_link_libraries(rviz_logo_marker ${catkin_LIBRARIES})
add_dependencies(tests rviz_logo_marker)
# This is an example node which publishes different kinds of point clouds.
add_executable(cloud_test EXCLUDE_FROM_ALL cloud_test.cpp)
+if(NOT WIN32)
+ set_target_properties(cloud_test PROPERTIES COMPILE_FLAGS "-std=c++11")
+endif()
target_link_libraries(cloud_test ${catkin_LIBRARIES})
add_dependencies(tests cloud_test)
# This is an example node which serves an interactive marker.
add_executable(interactive_marker_test interactive_marker_test.cpp)
+if(NOT WIN32)
+ set_target_properties(interactive_marker_test PROPERTIES COMPILE_FLAGS "-std=c++11")
+endif()
target_link_libraries(interactive_marker_test ${catkin_LIBRARIES})
add_dependencies(tests interactive_marker_test)
# This is another example node that publishes images.
add_executable(image_test EXCLUDE_FROM_ALL image_test.cpp)
+if(NOT WIN32)
+ set_target_properties(image_test PROPERTIES COMPILE_FLAGS "-std=c++11")
+endif()
target_link_libraries(image_test ${catkin_LIBRARIES})
add_dependencies(tests image_test)
# This is a node that sends lots of point clouds.
add_executable(send_lots_of_points EXCLUDE_FROM_ALL send_lots_of_points_node.cpp)
+if(NOT WIN32)
+ set_target_properties(send_lots_of_points PROPERTIES COMPILE_FLAGS "-std=c++11")
+endif()
target_link_libraries(send_lots_of_points ${catkin_LIBRARIES})
add_dependencies(tests send_lots_of_points)
# Yet another example which sends point clouds.
add_executable(send_point_cloud_2 EXCLUDE_FROM_ALL send_point_cloud_2.cpp)
+if(NOT WIN32)
+ set_target_properties(send_point_cloud_2 PROPERTIES COMPILE_FLAGS "-std=c++11")
+endif()
target_link_libraries(send_point_cloud_2 ${catkin_LIBRARIES})
add_dependencies(tests send_point_cloud_2)
# This is a node which sends grid cells.
add_executable(send_grid_cells EXCLUDE_FROM_ALL send_grid_cells_node.cpp)
+if(NOT WIN32)
+ set_target_properties(send_grid_cells PROPERTIES COMPILE_FLAGS "-std=c++11")
+endif()
target_link_libraries(send_grid_cells ${catkin_LIBRARIES})
add_dependencies(tests send_grid_cells)
# This is a test program that uses the rviz panel interface.
add_executable(render_panel_test render_panel_test.cpp)
-target_link_libraries(render_panel_test rviz ${catkin_LIBRARIES} ${QT_LIBRARIES})
-if (UseQt5)
- target_link_libraries(render_panel_test Qt5::Widgets)
+if(NOT WIN32)
+ set_target_properties(render_panel_test PROPERTIES COMPILE_FLAGS "-std=c++11")
endif()
+target_link_libraries(render_panel_test rviz ${catkin_LIBRARIES} ${QT_LIBRARIES})
add_dependencies(tests render_panel_test)
# This is an executable which uses the rviz new display diaglog interface.
add_executable(new_display_dialog_test new_display_dialog_test.cpp)
-target_link_libraries(new_display_dialog_test rviz ${catkin_LIBRARIES} ${QT_LIBRARIES})
-if (UseQt5)
- target_link_libraries(new_display_dialog_test Qt5::Widgets)
+if(NOT WIN32)
+ set_target_properties(new_display_dialog_test PROPERTIES COMPILE_FLAGS "-std=c++11")
endif()
+target_link_libraries(new_display_dialog_test rviz ${catkin_LIBRARIES} ${QT_LIBRARIES})
add_dependencies(tests new_display_dialog_test)
# This is an executable which uses the rviz color editor test.
add_executable(color_editor_test color_editor_test.cpp)
-target_link_libraries(color_editor_test rviz ${catkin_LIBRARIES} ${QT_LIBRARIES})
-if (UseQt5)
- target_link_libraries(color_editor_test Qt5::Widgets)
+if(NOT WIN32)
+ set_target_properties(color_editor_test PROPERTIES COMPILE_FLAGS "-std=c++11")
endif()
+target_link_libraries(color_editor_test rviz ${catkin_LIBRARIES} ${QT_LIBRARIES})
add_dependencies(tests color_editor_test)
# This is a modified version of the property test.
@@ -112,56 +139,48 @@ catkin_add_gtest(property_with_ros_spinner_test
property_test.cpp
ros_spinner.cpp
mock_property_change_receiver.cpp
- ${PROPERTY_TEST_MOC_FILES}
${MOC_MOCK_PROPERTY_CHANGE_RECEIVER}
)
target_link_libraries(property_with_ros_spinner_test rviz ${catkin_LIBRARIES} ${QT_LIBRARIES})
-if (UseQt5)
- target_link_libraries(property_with_ros_spinner_test Qt5::Widgets)
-endif()
add_dependencies(tests property_with_ros_spinner_test)
# This is an executable that uses the line_edit_with_button property interface.
add_executable(line_edit_with_button_test line_edit_with_button_test.cpp)
-target_link_libraries(line_edit_with_button_test rviz ${catkin_LIBRARIES} ${QT_LIBRARIES})
-if (UseQt5)
- target_link_libraries(line_edit_with_button_test Qt5::Widgets)
+if(NOT WIN32)
+ set_target_properties(line_edit_with_button_test PROPERTIES COMPILE_FLAGS "-std=c++11")
endif()
+target_link_libraries(line_edit_with_button_test rviz ${catkin_LIBRARIES} ${QT_LIBRARIES})
add_dependencies(tests line_edit_with_button_test)
# This is an executable which tests the connect/disconnect behavior of signals and slots in Qt.
-add_executable(connect_test connect_test.cpp ${MOC_FILES})
-target_link_libraries(connect_test ${QT_LIBRARIES})
-if (UseQt5)
- target_link_libraries(connect_test Qt5::Widgets)
+add_executable(connect_test connect_test.cpp)
+if(NOT WIN32)
+ set_target_properties(connect_test PROPERTIES COMPILE_FLAGS "-std=c++11")
endif()
+target_link_libraries(connect_test ${QT_LIBRARIES})
add_dependencies(tests connect_test)
# This is a GTest which tests the display configuration.
catkin_add_gtest(config_test config_test.cpp ../rviz/uniform_string_stream.cpp ../rviz/config.cpp)
-if (UseQt5)
- target_link_libraries(config_test Qt5::Widgets)
-endif()
target_link_libraries(config_test ${QT_LIBRARIES})
# This is an acceptance test executable which renders points.
add_executable(render_points_test
render_points_test.cpp
../rviz/ogre_helpers/orbit_camera.cpp
- ${RENDER_POINTS_TEST_MOC_FILES}
)
-target_link_libraries(render_points_test rviz ${catkin_LIBRARIES} ${QT_LIBRARIES})
-if (UseQt5)
- target_link_libraries(render_points_test Qt5::Widgets)
+if(NOT WIN32)
+ set_target_properties(render_points_test PROPERTIES COMPILE_FLAGS "-std=c++11")
endif()
+target_link_libraries(render_points_test rviz ${catkin_LIBRARIES} ${QT_LIBRARIES})
add_dependencies(tests render_points_test)
# This is an example application which creates two ogre render windows.
add_executable(two_render_widgets two_render_widgets.cpp)
-target_link_libraries(two_render_widgets rviz ${catkin_LIBRARIES} ${QT_LIBRARIES})
-if (UseQt5)
- target_link_libraries(two_render_widgets Qt5::Widgets)
+if(NOT WIN32)
+ set_target_properties(two_render_widgets PROPERTIES COMPILE_FLAGS "-std=c++11")
endif()
+target_link_libraries(two_render_widgets rviz ${catkin_LIBRARIES} ${QT_LIBRARIES})
add_dependencies(tests two_render_widgets)
# This is a GTest which tests the STL loader
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/debian-science/packages/ros/ros-rviz.git
More information about the debian-science-commits
mailing list