39 #include <OgreSceneManager.h> 40 #include <OgreSceneNode.h> 53 "Distance, in meters from the last arrow dropped, " 54 "that will cause a new arrow to drop.",
59 "Angular distance from the last arrow dropped, " 60 "that will cause a new arrow to drop.",
65 "Number of arrows to keep before removing the oldest. 0 means keep all of them.",
75 "Color of the arrows.",
130 D_Arrow::iterator it =
arrows_.begin();
131 D_Arrow::iterator end =
arrows_.end();
132 for ( ; it != end; ++it )
141 D_Axes::iterator it_axes =
axes_.begin();
142 D_Axes::iterator end_axes =
axes_.end();
143 for ( ; it_axes != end_axes; ++it_axes )
158 float red = color.redF();
159 float green = color.greenF();
160 float blue = color.blueF();
163 D_Arrow::iterator it =
arrows_.begin();
164 D_Arrow::iterator end =
arrows_.end();
165 for( ; it != end; ++it )
168 arrow->
setColor( red, green, blue, alpha );
175 D_Arrow::iterator it =
arrows_.begin();
176 D_Arrow::iterator end =
arrows_.end();
177 for ( ; it != end; ++it )
186 D_Axes::iterator it =
axes_.begin();
187 D_Axes::iterator end =
axes_.end();
188 for ( ; it != end; ++it )
232 D_Arrow::iterator it =
arrows_.begin();
233 D_Arrow::iterator end =
arrows_.end();
234 for ( ; it != end; ++it )
236 (*it)->getSceneNode()->setVisible( use_arrow );
239 D_Axes::iterator it_axes =
axes_.begin();
240 D_Axes::iterator end_axes =
axes_.end();
241 for ( ; it_axes != end_axes; ++it_axes )
243 (*it_axes)->getSceneNode()->setVisible( !use_arrow );
270 "This warning will only be output once but may be true for others; " 271 "enable DEBUG messages for ros.rviz.quaternions to see more details.",
273 ROS_DEBUG_NAMED(
"quaternions",
"Odometry '%s' contains unnormalized quaternions.",
280 Ogre::Vector3 current_position(message->pose.pose.position.x, message->pose.pose.position.y, message->pose.pose.position.z);
282 Eigen::Quaternionf current_orientation(message->pose.pose.orientation.w, message->pose.pose.orientation.x, message->pose.pose.orientation.y, message->pose.pose.orientation.z);
291 Ogre::Vector3 position;
292 Ogre::Quaternion orientation;
295 ROS_ERROR(
"Error transforming odometry '%s' from frame '%s' to frame '%s'",
318 arrow->setPosition( position );
319 arrow->setOrientation( orientation * Ogre::Quaternion( Ogre::Degree( -90 ), Ogre::Vector3::UNIT_Y ));
322 cov->setPosition( position );
323 cov->setOrientation( orientation );
328 arrow->setColor( color.redF(), color.greenF(), color.blueF(), alpha);
331 cov->setCovariance(message->pose);
335 arrow->getSceneNode()->setVisible( use_arrow );
339 axes_.push_back( axes );
359 delete axes_.front();
virtual QColor getColor() const
virtual void onInitialize()
Override this function to do subclass-specific initialization.
nav_msgs::Odometry::ConstPtr last_used_message_
virtual void onInitialize()
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
void updateShapeVisibility()
virtual int getInt() const
Return the internal property value as an integer.
virtual void processMessage(const nav_msgs::Odometry::ConstPtr &message)
rviz::FloatProperty * head_length_property_
virtual float getFloat() const
void updateColorAndAlpha()
rviz::FloatProperty * axes_radius_property_
Property specialized to enforce floating point max/min.
void set(float shaft_length, float shaft_diameter, float head_length, float head_diameter)
Set the parameters for this arrow.
Property specialized to provide max/min enforcement for integers.
Ogre::SceneNode * scene_node_
The Ogre::SceneNode to hold all 3D scene elements shown by this Display.
rviz::FloatProperty * position_tolerance_property_
CovarianceVisualPtr createAndPushBackVisual(Ogre::SceneManager *scene_manager, Ogre::SceneNode *parent_node)
virtual void setPosition(const Ogre::Vector3 &position)
Set the position of this object.
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
rviz::FloatProperty * shaft_length_property_
#define ROS_DEBUG_NAMED(name,...)
rviz::EnumProperty * shape_property_
bool validateFloats(const sensor_msgs::CameraInfo &msg)
virtual void addOption(const QString &option, int value=0)
void queueRender()
Convenience function which calls context_->queueRender().
Accumulates and displays the pose from a nav_msgs::Odometry message.
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].
rviz::FloatProperty * angle_tolerance_property_
bool transform(const Header &header, const geometry_msgs::Pose &pose, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
Transform a pose from a frame into the fixed frame.
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
void updateGeometry(rviz::Arrow *arrow)
CovarianceProperty * covariance_property_
#define ROS_WARN_ONCE_NAMED(name,...)
An object that displays a set of X/Y/Z axes, with X=Red, Y=Green, Z=Blue.
virtual void reset()
Called to tell the display to clear its state.
virtual ~OdometryDisplay()
Ogre::SceneManager * scene_manager_
A convenience variable equal to context_->getSceneManager().
virtual void setHidden(bool hidden)
Hide or show this property in any PropertyTreeWidget viewing its parent.
virtual void queueRender()=0
Queues a render. Multiple calls before a render happens will only cause a single render.
rviz::IntProperty * keep_property_
bool validateQuaternions(const visualization_msgs::InteractiveMarker &marker)
Property specialized to provide getter for booleans.
rviz::FloatProperty * alpha_property_
An arrow consisting of a cylinder and a cone.
void set(float length, float radius)
Set the parameters on this object.
virtual void setOrientation(const Ogre::Quaternion &orientation)
Set the orientation of the object.
virtual void update(float wall_dt, float ros_dt)
Called periodically by the visualization manager.
virtual void onEnable()
Overridden from MessageFilterDisplay to get Arrow/Axes visibility correct.
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
rviz::FloatProperty * axes_length_property_
bool initialized() const
Returns true if the display has been initialized.
Ogre::SceneNode * getSceneNode()
Get the scene node associated with this object.
rviz::ColorProperty * color_property_
virtual int getOptionInt()
Return the int value of the currently-chosen option, or 0 if the current option string does not have ...
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
rviz::FloatProperty * head_radius_property_
void updateAxisGeometry()
void updateArrowsGeometry()
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)
Show status level and text. This is thread-safe.
virtual QString getName() const
Return the name of this Property as a QString.
rviz::FloatProperty * shaft_radius_property_