30 #include <boost/bind.hpp> 32 #include <OgreSceneNode.h> 33 #include <OgreSceneManager.h> 34 #include <OgreManualObject.h> 35 #include <OgreBillboardSet.h> 36 #include <OgreMatrix4.h> 59 "The rendering operation to use to draw the grid lines.",
66 "The width, in meters, of each path line." 67 "Only works with the 'Billboards' style.",
73 "Color to draw the path.",
this );
76 "Amount of transparency to apply to the path.",
this );
79 "Number of paths to display.",
84 "Allows you to offset the path from the origin of the reference frame. In meters.",
94 "Length of the axes.",
97 "Radius of the axes.",
101 QColor( 255, 85, 255 ),
102 "Color to draw the poses.",
105 "Length of the arrow shaft.",
109 "Length of the arrow head.",
113 "Diameter of the arrow shaft.",
117 "Diameter of the arrow head.",
151 if (num > axes_vect.size()) {
152 for (
size_t i = axes_vect.size(); i < num; i++) {
156 axes_vect.push_back(axes);
159 else if (num < axes_vect.size()) {
160 for (
int i = axes_vect.size() - 1; num <= i; i--) {
163 axes_vect.resize(num);
169 if (num > arrow_vect.size()) {
170 for (
size_t i = arrow_vect.size(); i < num; i++) {
172 arrow_vect.push_back(arrow);
175 else if (num < arrow_vect.size()) {
176 for (
int i = arrow_vect.size() - 1; num <= i; i--) {
177 delete arrow_vect[i];
179 arrow_vect.resize(num);
229 if( billboard_line ) billboard_line->
setLineWidth( line_width );
280 std::vector<rviz::Axes*>& axes_vect =
axes_chain_[i];
281 for(
size_t j = 0; j < axes_vect.size() ; j++)
296 std::vector<rviz::Arrow*>& arrow_vect =
arrow_chain_[i];
297 for(
size_t j = 0; j < arrow_vect.size(); j++ )
299 arrow_vect[j]->setColor( color.redF(), color.greenF(), color.blueF(), 1.0f );
309 std::vector<rviz::Arrow*>& arrow_vect =
arrow_chain_[i];
310 for(
size_t j = 0; j < arrow_vect.size(); j++ )
329 manual_object->clear();
331 manual_object =
NULL;
341 delete billboard_line;
342 billboard_line =
NULL;
367 Ogre::ManualObject* manual_object =
scene_manager_->createManualObject();
368 manual_object->setDynamic(
true );
403 Ogre::ManualObject* manual_object =
NULL;
411 manual_object->clear();
416 billboard_line->
clear();
430 "This warning will only be output once but may be true for others; " 431 "enable DEBUG messages for ros.rviz.quaternions to see more details.",
433 ROS_DEBUG_NAMED(
"quaternions",
"Path '%s' contains unnormalized quaternions.",
438 Ogre::Vector3 position;
439 Ogre::Quaternion orientation;
442 ROS_DEBUG(
"Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable(
fixed_frame_ ));
445 Ogre::Matrix4 transform( orientation );
446 transform.setTrans( position );
454 uint32_t num_points = msg->poses.size();
460 manual_object->estimateVertexCount( num_points );
461 manual_object->begin(
"BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP );
462 for( uint32_t i=0; i < num_points; ++i)
464 const geometry_msgs::Point& pos = msg->poses[ i ].pose.position;
465 Ogre::Vector3 xpos = transform * Ogre::Vector3( pos.x, pos.y, pos.z );
466 manual_object->position( xpos.x, xpos.y, xpos.z );
467 manual_object->colour( color );
470 manual_object->end();
478 for( uint32_t i=0; i < num_points; ++i)
480 const geometry_msgs::Point& pos = msg->poses[ i ].pose.position;
481 Ogre::Vector3 xpos = transform * Ogre::Vector3( pos.x, pos.y, pos.z );
482 billboard_line->
addPoint( xpos, color );
490 std::vector<rviz::Arrow*>& arrow_vect =
arrow_chain_[ bufferIndex ];
491 std::vector<rviz::Axes*>& axes_vect =
axes_chain_[ bufferIndex ];
497 for( uint32_t i=0; i < num_points; ++i)
499 const geometry_msgs::Point& pos = msg->poses[ i ].pose.position;
500 const geometry_msgs::Quaternion& quat = msg->poses[ i ].pose.orientation;
501 Ogre::Vector3 xpos = transform * Ogre::Vector3( pos.x, pos.y, pos.z );
502 Ogre::Quaternion xquat = orientation * Ogre::Quaternion( quat.w, quat.x, quat.y, quat.z );
503 axes_vect[i]->setPosition(xpos);
504 axes_vect[i]->setOrientation(xquat);
510 for( uint32_t i=0; i < num_points; ++i)
512 const geometry_msgs::Point& pos = msg->poses[ i ].pose.position;
513 const geometry_msgs::Quaternion& quat = msg->poses[ i ].pose.orientation;
514 Ogre::Vector3 xpos = transform * Ogre::Vector3( pos.x, pos.y, pos.z );
515 Ogre::Quaternion xquat = orientation * Ogre::Quaternion( quat.w, quat.x, quat.y, quat.z );
518 arrow_vect[i]->setColor( color.redF(), color.greenF(), color.blueF(), 1.0f );
524 arrow_vect[i]->setPosition(xpos);
525 arrow_vect[i]->setDirection(xquat * Ogre::Vector3(1, 0, 0));
virtual QColor getColor() const
std::vector< Ogre::ManualObject * > manual_objects_
Ogre::ColourValue getOgreColor() const
std::vector< rviz::BillboardLine * > billboard_lines_
void addPoint(const Ogre::Vector3 &point)
virtual void onInitialize()
void updatePoseArrowColor()
FloatProperty * line_width_property_
ColorProperty * pose_arrow_color_property_
void allocateAxesVector(std::vector< rviz::Axes * > &axes_vect, int num)
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
An object that displays a multi-segment line strip rendered as billboards.
EnumProperty * style_property_
FloatProperty * pose_arrow_shaft_diameter_property_
FloatProperty * pose_arrow_head_diameter_property_
virtual int getInt() const
Return the internal property value as an integer.
FloatProperty * pose_arrow_shaft_length_property_
virtual float getFloat() const
VectorProperty * offset_property_
FloatProperty * pose_axes_radius_property_
Property specialized to enforce floating point max/min.
ColorProperty * color_property_
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.
Displays a nav_msgs::Path message.
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
#define ROS_DEBUG_NAMED(name,...)
virtual void reset()
Overridden from Display.
bool validateFloats(const sensor_msgs::CameraInfo &msg)
virtual void addOption(const QString &option, int value=0)
void show()
Show this Property in any PropertyTreeWidgets.
std::vector< std::vector< rviz::Arrow * > > arrow_chain_
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
void updatePoseArrowGeometry()
uint32_t messages_received_
void setNumLines(uint32_t num)
#define ROS_WARN_ONCE_NAMED(name,...)
An object that displays a set of X/Y/Z axes, with X=Red, Y=Green, Z=Blue.
void allocateArrowVector(std::vector< rviz::Arrow * > &arrow_vect, int num)
void setMaxPointsPerLine(uint32_t max)
Ogre::SceneManager * scene_manager_
A convenience variable equal to context_->getSceneManager().
FloatProperty * alpha_property_
void processMessage(const nav_msgs::Path::ConstPtr &msg)
Overridden from MessageFilterDisplay.
virtual void queueRender()=0
Queues a render. Multiple calls before a render happens will only cause a single render.
virtual Ogre::Vector3 getVector() const
IntProperty * buffer_length_property_
bool validateQuaternions(const visualization_msgs::InteractiveMarker &marker)
std::vector< std::vector< rviz::Axes * > > axes_chain_
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
Return the pose for a header, relative to the fixed frame, in Ogre classes.
EnumProperty * pose_style_property_
An arrow consisting of a cylinder and a cone.
void destroyPoseArrowChain()
void destroyPoseAxesChain()
void hide()
Hide this Property in any PropertyTreeWidgets.
void updatePoseAxisGeometry()
void updateBufferLength()
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)
FloatProperty * pose_arrow_head_length_property_
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.
FloatProperty * pose_axes_length_property_
void setLineWidth(float width)
virtual void onInitialize()
Overridden from Display.