30 #include <OgreEntity.h> 31 #include <OgreSceneNode.h> 53 #include <Eigen/Dense> 107 aabbs.push_back(
display_->
covariance_->getPositionShape()->getEntity()->getWorldBoundingBox() );
119 void setMessage(
const geometry_msgs::PoseWithCovarianceStampedConstPtr& message)
129 message->pose.pose.position.y,
130 message->pose.pose.position.z ));
132 message->pose.pose.orientation.x,
133 message->pose.pose.orientation.y,
134 message->pose.pose.orientation.z ));
136 message->pose.covariance[1+1*6],
137 message->pose.covariance[2+2*6] ));
140 message->pose.covariance[4+4*6],
141 message->pose.covariance[5+5*6] ));
156 : pose_valid_( false )
220 coll_handler_->addTrackedObjects( covariance_->getPositionSceneNode() );
221 coll_handler_->addTrackedObjects( covariance_->getOrientationSceneNode() );
311 ROS_WARN_ONCE_NAMED(
"quaternions",
"PoseWithCovariance '%s' contains unnormalized quaternions. " 312 "This warning will only be output once but may be true for others; " 313 "enable DEBUG messages for ros.rviz.quaternions to see more details.",
315 ROS_DEBUG_NAMED(
"quaternions",
"PoseWithCovariance '%s' contains unnormalized quaternions.",
319 Ogre::Vector3 position;
320 Ogre::Quaternion orientation;
323 ROS_ERROR(
"Error transforming pose '%s' from frame '%s' to frame '%s'",
335 arrow_->
setOrientation( orientation * Ogre::Quaternion( Ogre::Degree( -90 ), Ogre::Vector3::UNIT_Y ) );
rviz::FloatProperty * head_length_property_
Displays the pose from a geometry_msgs::PoseWithCovarianceStamped message.
rviz::FloatProperty * axes_length_property_
void updateArrowGeometry()
Ogre::ColourValue getOgreColor() const
virtual void processMessage(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &message)
virtual bool setVector(const Ogre::Vector3 &vector)
virtual void onInitialize()
StringProperty * frame_property_
bool getOrientationBool()
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
A single element of a property tree, with a name, value, description, and possibly children...
void updateShapeVisibility()
virtual float getFloat() const
PoseWithCovarianceDisplay()
virtual void setReadOnly(bool read_only)
Overridden from Property to propagate read-only-ness to children.
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.
rviz::EnumProperty * shape_property_
Ogre::SceneNode * scene_node_
The Ogre::SceneNode to hold all 3D scene elements shown by this Display.
rviz::FloatProperty * axes_radius_property_
CovarianceVisualPtr createAndPushBackVisual(Ogre::SceneManager *scene_manager, Ogre::SceneNode *parent_node)
Ogre::SceneNode * getSceneNode()
Get the scene node associated with this arrow.
virtual void setPosition(const Ogre::Vector3 &position)
Set the position of this object.
virtual bool getBool() const
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
#define ROS_DEBUG_NAMED(name,...)
CovarianceProperty * covariance_property_
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...
std::vector< Ogre::AxisAlignedBox > V_AABB
PoseWithCovarianceDisplay * display_
bool validateFloats(const sensor_msgs::CameraInfo &msg)
virtual void addOption(const QString &option, int value=0)
boost::shared_ptr< CovarianceVisual > covariance_
rviz::FloatProperty * head_radius_property_
void queueRender()
Convenience function which calls context_->queueRender().
virtual void setReadOnly(bool read_only)
Overridden from Property to propagate read-only-ness to children.
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 bool setQuaternion(const Ogre::Quaternion &quaternion)
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.
virtual void setOrientation(const Ogre::Quaternion &orientation)
Set the orientation.
PoseWithCovarianceDisplaySelectionHandler(PoseWithCovarianceDisplay *display, DisplayContext *context)
void setMessage(const geometry_msgs::PoseWithCovarianceStampedConstPtr &message)
virtual void reset()
Called to tell the display to clear its state.
QuaternionProperty * orientation_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.
Ogre::SceneManager * scene_manager_
A convenience variable equal to context_->getSceneManager().
rviz::FloatProperty * alpha_property_
PoseWithCovarianceDisplaySelectionHandlerPtr coll_handler_
virtual void onInitialize()
Override this function to do subclass-specific initialization.
virtual void setHidden(bool hidden)
Hide or show this property in any PropertyTreeWidget viewing its parent.
rviz::ColorProperty * color_property_
Property specialized for string values.
virtual ~PoseWithCovarianceDisplay()
void getAABBs(const Picked &obj, V_AABB &aabbs)
virtual void queueRender()=0
Queues a render. Multiple calls before a render happens will only cause a single render.
VectorProperty * covariance_orientation_property_
void updateColorAndAlpha()
void updateAxisGeometry()
bool validateQuaternions(const visualization_msgs::InteractiveMarker &marker)
Property specialized to provide getter for booleans.
void createProperties(const Picked &obj, Property *parent_property)
Override to create properties of the given picked object(s).
rviz::FloatProperty * shaft_length_property_
QList< Property * > properties_
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 onEnable()
Overridden from MessageFilterDisplay to get Arrow/Axes visibility correct.
bool initialized() const
Returns true if the display has been initialized.
Ogre::SceneNode * getSceneNode()
Get the scene node associated with this object.
VectorProperty * covariance_position_property_
rviz::FloatProperty * shaft_radius_property_
virtual void setReadOnly(bool read_only)
Prevent or allow users to edit this property from a PropertyTreeWidget.
virtual int getOptionInt()
Return the int value of the currently-chosen option, or 0 if the current option string does not have ...
bool setStdString(const std::string &std_str)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
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.
friend class PoseWithCovarianceDisplaySelectionHandler
VectorProperty * position_property_
Ogre::Entity * getEntity()