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 <OgreVector3.h>
00031 #include <OgreQuaternion.h>
00032 #include <OgreSceneNode.h>
00033 #include <OgreSceneManager.h>
00034 #include <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 ), last_arrow_set_from_points_(false)
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::setDefaultProportions()
00064 {
00065 arrow_->set(0.77, 1.0, 0.23, 2.0);
00066 }
00067
00068 void ArrowMarker::onNewMessage(const MarkerConstPtr& old_message, const MarkerConstPtr& new_message)
00069 {
00070 ROS_ASSERT(new_message->type == visualization_msgs::Marker::ARROW);
00071
00072 if (!new_message->points.empty() && new_message->points.size() < 2)
00073 {
00074 std::stringstream ss;
00075 ss << "Arrow marker [" << getStringID() << "] only specified one point of a point to point arrow.";
00076 if ( owner_ )
00077 {
00078 owner_->setMarkerStatus(getID(), StatusProperty::Error, ss.str());
00079 }
00080 ROS_DEBUG("%s", ss.str().c_str());
00081
00082 delete arrow_;
00083 arrow_ = 0;
00084
00085 return;
00086 }
00087
00088 if (!arrow_)
00089 {
00090 arrow_ = new Arrow(context_->getSceneManager(), child_scene_node_);
00091 setDefaultProportions();
00092 handler_.reset( new MarkerSelectionHandler( this, MarkerID( new_message->ns, new_message->id ), context_ ));
00093 handler_->addTrackedObjects( arrow_->getSceneNode() );
00094 }
00095
00096 Ogre::Vector3 pos, scale;
00097 Ogre::Quaternion orient;
00098 transform(new_message, pos, orient, scale);
00099 setPosition(pos);
00100 setOrientation( orient );
00101
00102 arrow_->setColor(new_message->color.r, new_message->color.g, new_message->color.b, new_message->color.a);
00103
00104
00105 if (new_message->points.size() == 2)
00106 {
00107 last_arrow_set_from_points_ = true;
00108
00109 Ogre::Vector3 point1( new_message->points[0].x, new_message->points[0].y, new_message->points[0].z );
00110 Ogre::Vector3 point2( new_message->points[1].x, new_message->points[1].y, new_message->points[1].z );
00111
00112 Ogre::Vector3 direction = point2 - point1;
00113 float distance = direction.length();
00114
00115 float head_length_proportion = 0.23;
00116 float head_length = head_length_proportion*distance;
00117 if ( new_message->scale.z != 0.0 )
00118 {
00119 float length = new_message->scale.z;
00120 head_length = std::max<double>(0.0, std::min<double>(length, distance));
00121 }
00122 float shaft_length = distance - head_length;
00123
00124 arrow_->set(shaft_length, new_message->scale.x, head_length, new_message->scale.y);
00125
00126 direction.normalise();
00127
00128
00129 Ogre::Quaternion orient = Ogre::Vector3::NEGATIVE_UNIT_Z.getRotationTo( direction );
00130
00131 arrow_->setPosition(point1);
00132 arrow_->setOrientation( orient );
00133 }
00134 else
00135 {
00136 if(last_arrow_set_from_points_)
00137 {
00138
00139 setDefaultProportions();
00140 last_arrow_set_from_points_ = false;
00141 }
00142 if ( owner_ && (new_message->scale.x * new_message->scale.y * new_message->scale.z == 0.0f) )
00143 {
00144 owner_->setMarkerStatus(getID(), StatusProperty::Warn, "Scale of 0 in one of x/y/z");
00145 }
00146 arrow_->setScale(scale);
00147
00148 Ogre::Quaternion orient = Ogre::Vector3::NEGATIVE_UNIT_Z.getRotationTo( Ogre::Vector3(1,0,0) );
00149 arrow_->setOrientation( orient );
00150 }
00151 }
00152
00153 S_MaterialPtr ArrowMarker::getMaterials()
00154 {
00155 S_MaterialPtr materials;
00156 extractMaterials( arrow_->getHead()->getEntity(), materials );
00157 extractMaterials( arrow_->getShaft()->getEntity(), materials );
00158 return materials;
00159 }
00160
00161 }