arrow_marker.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "arrow_marker.h"
00031 #include "marker_selection_handler.h"
00032 #include "rviz/default_plugin/marker_display.h"
00033 
00034 #include "rviz/visualization_manager.h"
00035 #include "rviz/selection/selection_manager.h"
00036 
00037 #include <rviz/ogre_helpers/arrow.h>
00038 #include <rviz/ogre_helpers/shape.h>
00039 
00040 #include <tf/transform_listener.h>
00041 
00042 #include <OGRE/OgreVector3.h>
00043 #include <OGRE/OgreQuaternion.h>
00044 #include <OGRE/OgreSceneNode.h>
00045 #include <OGRE/OgreSceneManager.h>
00046 #include <OGRE/OgreEntity.h>
00047 
00048 namespace rviz
00049 {
00050 
00051 ArrowMarker::ArrowMarker(MarkerDisplay* owner, VisualizationManager* manager, Ogre::SceneNode* parent_node)
00052 : MarkerBase(owner, manager, parent_node)
00053 , arrow_(0)
00054 {
00055   child_scene_node_ = scene_node_->createChildSceneNode();
00056 }
00057 
00058 ArrowMarker::~ArrowMarker()
00059 {
00060   delete arrow_;
00061   vis_manager_->getSceneManager()->destroySceneNode( child_scene_node_ );
00062 }
00063 
00064 void ArrowMarker::onNewMessage(const MarkerConstPtr& old_message, const MarkerConstPtr& new_message)
00065 {
00066   ROS_ASSERT(new_message->type == visualization_msgs::Marker::ARROW);
00067 
00068   if (!new_message->points.empty() && new_message->points.size() < 2)
00069   {
00070     std::stringstream ss;
00071     ss << "Arrow marker [" << getStringID() << "] only specified one point of a point to point arrow.";
00072     if ( owner_ )
00073     {
00074       owner_->setMarkerStatus(getID(), status_levels::Error, ss.str());
00075     }
00076     ROS_DEBUG("%s", ss.str().c_str());
00077 
00078     delete arrow_;
00079     arrow_ = 0;
00080 
00081     return;
00082   }
00083 
00084   if (!arrow_)
00085   {
00086     arrow_ = new Arrow(vis_manager_->getSceneManager(), child_scene_node_);
00087     vis_manager_->getSelectionManager()->removeObject(coll_);
00088     coll_ = vis_manager_->getSelectionManager()->createCollisionForObject(arrow_, SelectionHandlerPtr(new MarkerSelectionHandler(this, MarkerID(new_message->ns, new_message->id))), coll_);
00089   }
00090 
00091   Ogre::Vector3 pos, scale;
00092   Ogre::Quaternion orient;
00093   transform(new_message, pos, orient, scale);
00094   setPosition(pos);
00095   setOrientation( orient );
00096 
00097   arrow_->setColor(new_message->color.r, new_message->color.g, new_message->color.b, new_message->color.a);
00098 
00099   // compute translation & rotation from the two points
00100   if (new_message->points.size() == 2)
00101   {
00102     Ogre::Vector3 point1( new_message->points[0].x, new_message->points[0].y, new_message->points[0].z );
00103     Ogre::Vector3 point2( new_message->points[1].x, new_message->points[1].y, new_message->points[1].z );
00104 
00105     Ogre::Vector3 direction = point2 - point1;
00106     float distance = direction.length();
00107 
00108     float head_length = 0.1*distance;
00109     if ( new_message->scale.z != 0.0 )
00110     {
00111       head_length = new_message->scale.z;
00112     }
00113     float shaft_length = distance - head_length;
00114 
00115     arrow_->set(shaft_length, new_message->scale.x, head_length, new_message->scale.y);
00116 
00117     direction.normalise();
00118 
00119     // for some reason the arrow goes into the y direction by default
00120     Ogre::Quaternion orient = Ogre::Vector3::NEGATIVE_UNIT_Z.getRotationTo( direction );
00121 
00122     arrow_->setPosition(point1);
00123     arrow_->setOrientation( orient );
00124   }
00125   else
00126   {
00127     if ( owner_ && (new_message->scale.x * new_message->scale.y * new_message->scale.z == 0.0f) )
00128     {
00129       owner_->setMarkerStatus(getID(), status_levels::Warn, "Scale of 0 in one of x/y/z");
00130     }
00131     arrow_->setScale(scale);
00132 
00133     Ogre::Quaternion orient = Ogre::Vector3::NEGATIVE_UNIT_Z.getRotationTo( Ogre::Vector3(1,0,0) );
00134     arrow_->setOrientation( orient );
00135   }
00136 }
00137 
00138 S_MaterialPtr ArrowMarker::getMaterials()
00139 {
00140   S_MaterialPtr materials;
00141   extractMaterials( arrow_->getHead()->getEntity(), materials );
00142   extractMaterials( arrow_->getShaft()->getEntity(), materials );
00143   return materials;
00144 }
00145 
00146 }


rviz
Author(s): Dave Hershberger, Josh Faust
autogenerated on Mon Jan 6 2014 11:54:32