pose_array_display.cpp
Go to the documentation of this file.
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 <OgreManualObject.h>
31 #include <OgreSceneManager.h>
32 #include <OgreSceneNode.h>
33 
34 #include "rviz/display_context.h"
35 #include "rviz/frame_manager.h"
39 #include "rviz/validate_floats.h"
41 #include "rviz/ogre_helpers/axes.h"
42 
43 #include "pose_array_display.h"
44 
45 namespace jsk_rviz_plugins
46 {
47 
49  : manual_object_( NULL )
50 {
51  color_property_ = new rviz::ColorProperty( "Color", QColor( 255, 25, 0 ), "Color to draw the arrows.", this );
52  length_property_ = new rviz::FloatProperty( "Arrow Length", 0.3, "Length of the arrows.", this );
53  axes_length_property_ = new rviz::FloatProperty( "Axes Length", 1, "Length of each axis, in meters.",
54  this, SLOT( updateAxisGeometry() ));
55  axes_radius_property_ = new rviz::FloatProperty( "Axes Radius", 0.1, "Radius of each axis, in meters.",
56  this, SLOT( updateAxisGeometry() ));
57  shape_property_ = new rviz::EnumProperty( "Shape", "Arrow", "Shape to display the pose as.",
58  this, SLOT( updateShapeChoice() ));
59  shape_property_->addOption( "Arrow", Arrow );
60  shape_property_->addOption( "Axes", Axes );
61 }
62 
64 {
65  if ( initialized() )
66  {
67  scene_manager_->destroyManualObject( manual_object_ );
68  }
69 }
70 
72 {
73 #if ROS_VERSION_MINIMUM(1,12,0)
74  ROS_WARN("jsk_rviz_plugins/PoseArrayDisplay is deprecated. Please use rviz default PoseArrayDisplay plugin instead.");
75 #endif
77  manual_object_ = scene_manager_->createManualObject();
78  manual_object_->setDynamic( true );
79  scene_node_->attachObject( manual_object_ );
80 
81  updateShapeChoice();
82  updateShapeVisibility();
83  updateAxisGeometry();
84 }
85 
87 {
88  bool use_arrow = ( shape_property_->getOptionInt() == Arrow );
89 
90  color_property_->setHidden( !use_arrow );
91  length_property_->setHidden( !use_arrow );
92 
93  axes_length_property_->setHidden( use_arrow );
94  axes_radius_property_->setHidden( use_arrow );
95 
96  updateShapeVisibility();
97 
99 }
100 
101 void PoseArrayDisplay::updateShapeVisibility()
102 {
103 
104  if( !pose_valid_ )
105  {
106  manual_object_->setVisible(false);
107  for (int i = 0; i < coords_nodes_.size() ; i++)
108  coords_nodes_[i]->setVisible(false);
109  }
110  else
111  {
112  bool use_arrow = (shape_property_->getOptionInt() == Arrow);
113  for (int i = 0; i < coords_nodes_.size() ; i++)
114  coords_nodes_[i]->setVisible(!use_arrow);
115 
116  manual_object_->setVisible(use_arrow);
117  }
118 }
119 
120 void PoseArrayDisplay::updateAxisGeometry()
121 {
122  for(size_t i = 0; i < coords_objects_.size() ; i++)
123  coords_objects_[i]->set( axes_length_property_->getFloat(),
124  axes_radius_property_->getFloat() );
126 }
127 
128 void PoseArrayDisplay::allocateCoords(int num)
129 {
130  if (num > coords_objects_.size()) {
131  for (size_t i = coords_objects_.size(); i < num; i++) {
132  Ogre::SceneNode* scene_node = scene_node_->createChildSceneNode();
133  rviz::Axes* axes = new rviz::Axes( scene_manager_, scene_node,
134  axes_length_property_->getFloat(),
135  axes_radius_property_->getFloat());
136  coords_nodes_.push_back(scene_node);
137  coords_objects_.push_back(axes);
138  }
139  }
140  else if (num < coords_objects_.size()) {
141  for (int i = coords_objects_.size() - 1; num <= i; i--) {
142  delete coords_objects_[i];
143  scene_manager_->destroySceneNode(coords_nodes_[i]);
144  }
145  coords_objects_.resize(num);
146  coords_nodes_.resize(num);
147  }
148 }
149 
150 
151 bool validateFloats( const geometry_msgs::PoseArray& msg )
152 {
153  return rviz::validateFloats( msg.poses );
154 }
155 
156 void PoseArrayDisplay::processMessage( const geometry_msgs::PoseArray::ConstPtr& msg )
157 {
158 #if ROS_VERSION_MINIMUM(1,12,0)
159  ROS_WARN_THROTTLE(1.0, "jsk_rviz_plugins/PoseArrayDisplay is deprecated. Please use rviz default PoseArrayDisplay plugin instead.");
160 #endif
161  if( !validateFloats( *msg ))
162  {
163  setStatus( rviz::StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
164  return;
165  }
166 
167  manual_object_->clear();
168 
169  Ogre::Vector3 position;
170  Ogre::Quaternion orientation;
171  if( !context_->getFrameManager()->getTransform( msg->header, position, orientation ))
172  {
173  ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
174  }
175 
176  pose_valid_ = true;
177  updateShapeVisibility();
178 
179  scene_node_->setPosition( position );
180  scene_node_->setOrientation( orientation );
181 
182  manual_object_->clear();
183 
184  if(shape_property_->getOptionInt() == Arrow ) {
185  for (int i = 0; i < coords_nodes_.size() ; i++)
186  coords_nodes_[i]->setVisible(false);
187  Ogre::ColourValue color = color_property_->getOgreColor();
188  float length = length_property_->getFloat();
189  size_t num_poses = msg->poses.size();
190  manual_object_->estimateVertexCount( num_poses * 6 );
191  manual_object_->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST );
192  for( size_t i=0; i < num_poses; ++i )
193  {
194  Ogre::Vector3 pos( msg->poses[i].position.x,
195  msg->poses[i].position.y,
196  msg->poses[i].position.z );
197  Ogre::Quaternion orient( msg->poses[i].orientation.w,
198  msg->poses[i].orientation.x,
199  msg->poses[i].orientation.y,
200  msg->poses[i].orientation.z );
201  // orient here is not normalized, so the scale of the quaternion
202  // will affect the scale of the arrow.
203 
204  Ogre::Vector3 vertices[6];
205  vertices[0] = pos; // back of arrow
206  vertices[1] = pos + orient * Ogre::Vector3( length, 0, 0 ); // tip of arrow
207  vertices[2] = vertices[ 1 ];
208  vertices[3] = pos + orient * Ogre::Vector3( 0.75*length, 0.2*length, 0 );
209  vertices[4] = vertices[ 1 ];
210  vertices[5] = pos + orient * Ogre::Vector3( 0.75*length, -0.2*length, 0 );
211 
212  for( int i = 0; i < 6; ++i )
213  {
214  manual_object_->position( vertices[i] );
215  manual_object_->colour( color );
216  }
217  }
218  manual_object_->end();
219  }
220  else{
221  allocateCoords(msg->poses.size());
222  for (int i = 0; i < msg->poses.size() ; i++){
223  geometry_msgs::Pose pose = msg->poses[i];
224  Ogre::SceneNode* scene_node = coords_nodes_[i];
225  scene_node->setVisible(true);
226 
227  Ogre::Vector3 position( msg->poses[i].position.x,
228  msg->poses[i].position.y,
229  msg->poses[i].position.z );
230  Ogre::Quaternion orientation( msg->poses[i].orientation.w,
231  msg->poses[i].orientation.x,
232  msg->poses[i].orientation.y,
233  msg->poses[i].orientation.z );
234  scene_node->setPosition(position);
235  scene_node->setOrientation(orientation); // scene node is at frame pose
236  }
237  }
238 
240 }
241 
243 {
244  MFDClass::reset();
245  if( manual_object_ )
246  {
247  manual_object_->clear();
248  }
249  if ( coords_objects_.size() > 0 ) {
250  allocateCoords(0);
251  }
252 
253 }
254 
255 } // namespace rviz
256 
msg
#define NULL
pose
#define ROS_WARN_THROTTLE(rate,...)
pos
Ogre::ColourValue getOgreColor() const
PLUGINLIB_EXPORT_CLASS(jsk_rviz_plugins::PictogramArrayDisplay, rviz::Display)
DisplayContext * context_
#define ROS_WARN(...)
Ogre::SceneNode * scene_node_
QString fixed_frame_
virtual void processMessage(const geometry_msgs::PoseArray::ConstPtr &msg)
bool validateFloats(const sensor_msgs::CameraInfo &msg)
virtual FrameManager * getFrameManager() const =0
bool validateFloats(const visualization_msgs::InteractiveMarker &msg)
Ogre::SceneManager * scene_manager_
virtual void setHidden(bool hidden)
virtual void queueRender()=0
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
bool initialized() const
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)
#define ROS_DEBUG(...)


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Sat Mar 20 2021 03:03:18