Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
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
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 }