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 <OGRE/OgreVector3.h>
00031 #include <OGRE/OgreQuaternion.h>
00032 #include <OGRE/OgreSceneNode.h>
00033 #include <OGRE/OgreSceneManager.h>
00034 #include <OGRE/OgreEntity.h>
00035
00036 #include <tf/transform_listener.h>
00037
00038 #include "rviz/default_plugin/marker_display.h"
00039 #include "rviz/default_plugin/markers/marker_selection_handler.h"
00040 #include "rviz/display_context.h"
00041 #include "rviz/ogre_helpers/arrow.h"
00042 #include "rviz/ogre_helpers/shape.h"
00043 #include "rviz/selection/selection_manager.h"
00044
00045 #include "rviz/default_plugin/markers/arrow_marker.h"
00046
00047 namespace rviz
00048 {
00049
00050 ArrowMarker::ArrowMarker( MarkerDisplay* owner, DisplayContext* context, Ogre::SceneNode* parent_node )
00051 : MarkerBase( owner, context, parent_node )
00052 , arrow_( 0 )
00053 {
00054 child_scene_node_ = scene_node_->createChildSceneNode();
00055 }
00056
00057 ArrowMarker::~ArrowMarker()
00058 {
00059 delete arrow_;
00060 context_->getSceneManager()->destroySceneNode( child_scene_node_ );
00061 }
00062
00063 void ArrowMarker::onNewMessage(const MarkerConstPtr& old_message, const MarkerConstPtr& new_message)
00064 {
00065 ROS_ASSERT(new_message->type == visualization_msgs::Marker::ARROW);
00066
00067 if (!new_message->points.empty() && new_message->points.size() < 2)
00068 {
00069 std::stringstream ss;
00070 ss << "Arrow marker [" << getStringID() << "] only specified one point of a point to point arrow.";
00071 if ( owner_ )
00072 {
00073 owner_->setMarkerStatus(getID(), StatusProperty::Error, ss.str());
00074 }
00075 ROS_DEBUG("%s", ss.str().c_str());
00076
00077 delete arrow_;
00078 arrow_ = 0;
00079
00080 return;
00081 }
00082
00083 if (!arrow_)
00084 {
00085 arrow_ = new Arrow(context_->getSceneManager(), child_scene_node_);
00086 context_->getSelectionManager()->removeObject(coll_);
00087 coll_ = context_->getSelectionManager()->createCollisionForObject(arrow_, SelectionHandlerPtr(new MarkerSelectionHandler(this, MarkerID(new_message->ns, new_message->id))), coll_);
00088 }
00089
00090 Ogre::Vector3 pos, scale;
00091 Ogre::Quaternion orient;
00092 transform(new_message, pos, orient, scale);
00093 setPosition(pos);
00094 setOrientation( orient );
00095
00096 arrow_->setColor(new_message->color.r, new_message->color.g, new_message->color.b, new_message->color.a);
00097
00098
00099 if (new_message->points.size() == 2)
00100 {
00101 Ogre::Vector3 point1( new_message->points[0].x, new_message->points[0].y, new_message->points[0].z );
00102 Ogre::Vector3 point2( new_message->points[1].x, new_message->points[1].y, new_message->points[1].z );
00103
00104 Ogre::Vector3 direction = point2 - point1;
00105 float distance = direction.length();
00106
00107 float head_length = 0.1*distance;
00108 if ( new_message->scale.z != 0.0 )
00109 {
00110 head_length = new_message->scale.z;
00111 }
00112 float shaft_length = distance - head_length;
00113
00114 arrow_->set(shaft_length, new_message->scale.x, head_length, new_message->scale.y);
00115
00116 direction.normalise();
00117
00118
00119 Ogre::Quaternion orient = Ogre::Vector3::NEGATIVE_UNIT_Z.getRotationTo( direction );
00120
00121 arrow_->setPosition(point1);
00122 arrow_->setOrientation( orient );
00123 }
00124 else
00125 {
00126 if ( owner_ && (new_message->scale.x * new_message->scale.y * new_message->scale.z == 0.0f) )
00127 {
00128 owner_->setMarkerStatus(getID(), StatusProperty::Warn, "Scale of 0 in one of x/y/z");
00129 }
00130 arrow_->setScale(scale);
00131
00132 Ogre::Quaternion orient = Ogre::Vector3::NEGATIVE_UNIT_Z.getRotationTo( Ogre::Vector3(1,0,0) );
00133 arrow_->setOrientation( orient );
00134 }
00135 }
00136
00137 S_MaterialPtr ArrowMarker::getMaterials()
00138 {
00139 S_MaterialPtr materials;
00140 extractMaterials( arrow_->getHead()->getEntity(), materials );
00141 extractMaterials( arrow_->getShaft()->getEntity(), materials );
00142 return materials;
00143 }
00144
00145 }