From bd42dddaf3c7e58baba22b1787fa991f3a9275b6 Mon Sep 17 00:00:00 2001 From: aginika Date: Tue, 3 Mar 2015 22:30:25 +0900 Subject: [PATCH] [jsk_rviz_plugins] add tmp pose array display --- jsk_rviz_plugins/CMakeLists.txt | 2 + jsk_rviz_plugins/plugin_description.xml | 6 + jsk_rviz_plugins/src/pose_array_display.cpp | 242 ++++++++++++++++++++ jsk_rviz_plugins/src/pose_array_display.h | 92 ++++++++ 4 files changed, 342 insertions(+) create mode 100644 jsk_rviz_plugins/src/pose_array_display.cpp create mode 100644 jsk_rviz_plugins/src/pose_array_display.h diff --git a/jsk_rviz_plugins/CMakeLists.txt b/jsk_rviz_plugins/CMakeLists.txt index b8f9ec413..4693a95d6 100644 --- a/jsk_rviz_plugins/CMakeLists.txt +++ b/jsk_rviz_plugins/CMakeLists.txt @@ -93,6 +93,7 @@ qt4_wrap_cpp(MOC_FILES src/tablet_controller_panel.h src/video_capture_display.h src/twist_stamped_display.h + src/pose_array_display.h ) set(SOURCE_FILES @@ -136,6 +137,7 @@ set(SOURCE_FILES src/tablet_controller_panel.cpp src/video_capture_display.cpp src/twist_stamped_display.cpp + src/pose_array_display.cpp ${MOC_FILES} ) diff --git a/jsk_rviz_plugins/plugin_description.xml b/jsk_rviz_plugins/plugin_description.xml index 3dacc65e1..dd90a1e66 100644 --- a/jsk_rviz_plugins/plugin_description.xml +++ b/jsk_rviz_plugins/plugin_description.xml @@ -22,6 +22,12 @@ + + + + diff --git a/jsk_rviz_plugins/src/pose_array_display.cpp b/jsk_rviz_plugins/src/pose_array_display.cpp new file mode 100644 index 000000000..20fc48038 --- /dev/null +++ b/jsk_rviz_plugins/src/pose_array_display.cpp @@ -0,0 +1,242 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include + +#include "rviz/display_context.h" +#include "rviz/frame_manager.h" +#include "rviz/properties/color_property.h" +#include "rviz/properties/float_property.h" +#include "rviz/properties/enum_property.h" +#include "rviz/validate_floats.h" +#include "rviz/ogre_helpers/arrow.h" +#include "rviz/ogre_helpers/axes.h" + +#include "pose_array_display.h" + +namespace jsk_rviz_plugins +{ + +PoseArrayDisplay::PoseArrayDisplay() + : manual_object_( NULL ) +{ + color_property_ = new rviz::ColorProperty( "Color", QColor( 255, 25, 0 ), "Color to draw the arrows.", this ); + length_property_ = new rviz::FloatProperty( "Arrow Length", 0.3, "Length of the arrows.", this ); + axes_length_property_ = new rviz::FloatProperty( "Axes Length", 1, "Length of each axis, in meters.", + this, SLOT( updateAxisGeometry() )); + axes_radius_property_ = new rviz::FloatProperty( "Axes Radius", 0.1, "Radius of each axis, in meters.", + this, SLOT( updateAxisGeometry() )); + shape_property_ = new rviz::EnumProperty( "Shape", "Arrow", "Shape to display the pose as.", + this, SLOT( updateShapeChoice() )); + shape_property_->addOption( "Arrow", Arrow ); + shape_property_->addOption( "Axes", Axes ); +} + +PoseArrayDisplay::~PoseArrayDisplay() +{ + if ( initialized() ) + { + scene_manager_->destroyManualObject( manual_object_ ); + } +} + +void PoseArrayDisplay::onInitialize() +{ + MFDClass::onInitialize(); + manual_object_ = scene_manager_->createManualObject(); + manual_object_->setDynamic( true ); + scene_node_->attachObject( manual_object_ ); + + updateShapeChoice(); + updateShapeVisibility(); + updateAxisGeometry(); +} + +void PoseArrayDisplay::updateShapeChoice() +{ + bool use_arrow = ( shape_property_->getOptionInt() == Arrow ); + + color_property_->setHidden( !use_arrow ); + length_property_->setHidden( !use_arrow ); + + axes_length_property_->setHidden( use_arrow ); + axes_radius_property_->setHidden( use_arrow ); + + updateShapeVisibility(); + + context_->queueRender(); +} + +void PoseArrayDisplay::updateShapeVisibility() +{ + + if( !pose_valid_ ) + { + manual_object_->setVisible(false); + for (int i = 0; i < coords_nodes_.size() ; i++) + coords_nodes_[i]->setVisible(false); + } + else + { + bool use_arrow = (shape_property_->getOptionInt() == Arrow); + for (int i = 0; i < coords_nodes_.size() ; i++) + coords_nodes_[i]->setVisible(!use_arrow); + + manual_object_->setVisible(use_arrow); + } +} + +void PoseArrayDisplay::updateAxisGeometry() +{ + for(size_t i = 0; i < coords_objects_.size() ; i++) + coords_objects_[i]->set( axes_length_property_->getFloat(), + axes_radius_property_->getFloat() ); + context_->queueRender(); +} + +void PoseArrayDisplay::allocateCoords(int num) +{ + if (num > coords_objects_.size()) { + for (size_t i = coords_objects_.size(); i < num; i++) { + Ogre::SceneNode* scene_node = scene_node_->createChildSceneNode(); + rviz::Axes* axes = new rviz::Axes( scene_manager_, scene_node, + axes_length_property_->getFloat(), + axes_radius_property_->getFloat()); + coords_nodes_.push_back(scene_node); + coords_objects_.push_back(axes); + } + } + else if (num < coords_objects_.size()) + { + coords_objects_.resize(num); + } +} + + +bool validateFloats( const geometry_msgs::PoseArray& msg ) +{ + return rviz::validateFloats( msg.poses ); +} + +void PoseArrayDisplay::processMessage( const geometry_msgs::PoseArray::ConstPtr& msg ) +{ + if( !validateFloats( *msg )) + { + setStatus( rviz::StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" ); + return; + } + + manual_object_->clear(); + + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if( !context_->getFrameManager()->getTransform( msg->header, position, orientation )) + { + ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable( fixed_frame_ )); + } + + pose_valid_ = true; + updateShapeVisibility(); + + scene_node_->setPosition( position ); + scene_node_->setOrientation( orientation ); + + manual_object_->clear(); + + if(shape_property_->getOptionInt() == Arrow ) { + for (int i = 0; i < coords_nodes_.size() ; i++) + coords_nodes_[i]->setVisible(false); + Ogre::ColourValue color = color_property_->getOgreColor(); + float length = length_property_->getFloat(); + size_t num_poses = msg->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. + + Ogre::Vector3 vertices[6]; + vertices[0] = pos; // back of arrow + vertices[1] = pos + orient * Ogre::Vector3( length, 0, 0 ); // tip of arrow + vertices[2] = vertices[ 1 ]; + vertices[3] = pos + orient * Ogre::Vector3( 0.75*length, 0.2*length, 0 ); + vertices[4] = vertices[ 1 ]; + vertices[5] = pos + orient * Ogre::Vector3( 0.75*length, -0.2*length, 0 ); + + for( int i = 0; i < 6; ++i ) + { + manual_object_->position( vertices[i] ); + manual_object_->colour( color ); + } + } + manual_object_->end(); + } + else{ + allocateCoords(msg->poses.size()); + for (int i = 0; i < msg->poses.size() ; i++){ + geometry_msgs::Pose pose = msg->poses[i]; + Ogre::SceneNode* scene_node = coords_nodes_[i]; + scene_node->setVisible(true); + + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if(!context_->getFrameManager()->transform(msg->header, pose, position, orientation)) { + return; + } + scene_node->setPosition(position); + scene_node->setOrientation(orientation); // scene node is at frame pose + } + } + + context_->queueRender(); +} + +void PoseArrayDisplay::reset() +{ + MFDClass::reset(); + if( manual_object_ ) + { + manual_object_->clear(); + } +} + +} // namespace rviz + +#include +PLUGINLIB_EXPORT_CLASS( jsk_rviz_plugins::PoseArrayDisplay, rviz::Display ) diff --git a/jsk_rviz_plugins/src/pose_array_display.h b/jsk_rviz_plugins/src/pose_array_display.h new file mode 100644 index 000000000..fed858be3 --- /dev/null +++ b/jsk_rviz_plugins/src/pose_array_display.h @@ -0,0 +1,92 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef RVIZ_POSE_ARRAY_DISPLAY_H_ +#define RVIZ_POSE_ARRAY_DISPLAY_H_ + +#include + +#include "rviz/message_filter_display.h" + +namespace Ogre +{ +class ManualObject; +}; + +namespace rviz +{ +class ColorProperty; +class FloatProperty; +class EnumProperty; +class Axes; +}; + +namespace jsk_rviz_plugins +{ +/** @brief Displays a geometry_msgs/PoseArray message as a bunch of line-drawn arrows. */ + class PoseArrayDisplay: public rviz::MessageFilterDisplay +{ +Q_OBJECT +public: + enum Shape + { + Arrow, + Axes, + }; + PoseArrayDisplay(); + virtual ~PoseArrayDisplay(); + + virtual void onInitialize(); + virtual void reset(); + +private Q_SLOTS: + void updateShapeChoice(); + void updateShapeVisibility(); + void updateAxisGeometry(); + void allocateCoords(int num); + +private: + virtual void processMessage( const geometry_msgs::PoseArray::ConstPtr& msg ); + + Ogre::ManualObject* manual_object_; + + rviz::ColorProperty* color_property_; + rviz::FloatProperty* length_property_; + rviz::FloatProperty* axes_length_property_; + rviz::FloatProperty* axes_radius_property_; + rviz::EnumProperty* shape_property_; + std::vector coords_objects_; + std::vector coords_nodes_; + + bool pose_valid_; +}; + +} // namespace rviz + +#endif /* RVIZ_POSE_ARRAY_DISPLAY_H_ */