30 #include <OgreVector3.h> 31 #include <OgreQuaternion.h> 32 #include <OgreSceneNode.h> 33 #include <OgreSceneManager.h> 34 #include <OgreEntity.h> 52 , arrow_( 0 ), last_arrow_set_from_points_(false)
70 ROS_ASSERT(new_message->type == visualization_msgs::Marker::ARROW);
72 if (!new_message->points.empty() && new_message->points.size() < 2)
88 Ogre::Vector3 pos, scale;
89 Ogre::Quaternion orient;
90 transform(new_message, pos, orient, scale);
94 arrow_->
setColor(new_message->color.r, new_message->color.g, new_message->color.b, new_message->color.a);
97 if (new_message->points.size() == 2)
101 Ogre::Vector3 point1( new_message->points[0].x, new_message->points[0].y, new_message->points[0].z );
102 Ogre::Vector3 point2( new_message->points[1].x, new_message->points[1].y, new_message->points[1].z );
104 Ogre::Vector3 direction = point2 - point1;
105 float distance = direction.length();
107 float head_length_proportion = 0.23;
108 float head_length = head_length_proportion*distance;
109 if ( new_message->scale.z != 0.0 )
111 float length = new_message->scale.z;
112 head_length = std::max<double>(0.0, std::min<double>(length, distance));
114 float shaft_length = distance - head_length;
116 arrow_->
set(shaft_length, new_message->scale.x, head_length, new_message->scale.y);
118 direction.normalise();
121 Ogre::Quaternion orient = Ogre::Vector3::NEGATIVE_UNIT_Z.getRotationTo( direction );
136 Ogre::Quaternion orient = Ogre::Vector3::NEGATIVE_UNIT_Z.getRotationTo( Ogre::Vector3(1,0,0) );
Ogre::SceneNode * scene_node_
bool transform(const MarkerConstPtr &message, Ogre::Vector3 &pos, Ogre::Quaternion &orient, Ogre::Vector3 &scale)
TFSIMD_FORCE_INLINE tfScalar distance(const Vector3 &v) const
virtual void setPosition(const Ogre::Vector3 &position)
void set(float shaft_length, float shaft_diameter, float head_length, float head_diameter)
Set the parameters for this arrow.
void extractMaterials(Ogre::Entity *entity, S_MaterialPtr &materials)
Ogre::SceneNode * getSceneNode()
Get the scene node associated with this arrow.
virtual void onNewMessage(const MarkerConstPtr &old_message, const MarkerConstPtr &new_message)
std::pair< std::string, int32_t > MarkerID
virtual void setPosition(const Ogre::Vector3 &position)
Set the position of the base of the arrow.
Pure-virtual base class for objects which give Display subclasses context in which to work...
boost::shared_ptr< MarkerSelectionHandler > handler_
virtual void setScale(const Ogre::Vector3 &scale)
Set the scale of the object. Always relative to the identity orientation of the object.
virtual void setColor(float r, float g, float b, float a)
Set the color of this arrow. Sets both the head and shaft color to the same value. Values are in the range [0, 1].
virtual void setOrientation(const Ogre::Quaternion &orientation)
Set the orientation.
DisplayContext * context_
bool last_arrow_set_from_points_
visualization_msgs::Marker::ConstPtr MarkerConstPtr
virtual Ogre::SceneManager * getSceneManager() const =0
Returns the Ogre::SceneManager used for the main RenderPanel.
virtual S_MaterialPtr getMaterials()
Ogre::SceneNode * child_scene_node_
std::set< Ogre::MaterialPtr > S_MaterialPtr
An arrow consisting of a cylinder and a cone.
virtual void setOrientation(const Ogre::Quaternion &orientation)
virtual void setDefaultProportions()
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
Displays "markers" sent in by other ROS nodes on the "visualization_marker" topic.
ArrowMarker(MarkerDisplay *owner, DisplayContext *context, Ogre::SceneNode *parent_node)
Ogre::Entity * getEntity()