30 #include <OgreManualObject.h> 31 #include <OgreSceneManager.h> 32 #include <OgreSceneNode.h> 61 Ogre::Vector3 vectorRosToOgre( geometry_msgs::Point
const & point )
63 return Ogre::Vector3( point.x, point.y, point.z );
66 Ogre::Quaternion quaternionRosToOgre( geometry_msgs::Quaternion
const & quaternion )
75 : manual_object_(
NULL )
143 "Message contained invalid floating point values (nans or infs)" );
149 ROS_WARN_ONCE_NAMED(
"quaternions",
"PoseArray msg received on topic '%s' contains unnormalized quaternions. " 150 "This warning will only be output once but may be true for others; " 151 "enable DEBUG messages for ros.rviz.quaternions to see more details.",
153 ROS_DEBUG_NAMED(
"quaternions",
"PoseArray msg received on topic '%s' contains unnormalized quaternions.",
163 poses_.resize( msg->poses.size() );
164 for (std::size_t i = 0; i < msg->poses.size(); ++i)
166 poses_[i].position = vectorRosToOgre( msg->poses[i].position );
167 poses_[i].orientation = quaternionRosToOgre( msg->poses[i].orientation );
176 Ogre::Vector3 position;
177 Ogre::Quaternion orientation;
180 ROS_ERROR(
"Error transforming pose '%s' from frame '%s' to frame '%s'",
181 qPrintable(
getName() ), header.frame_id.c_str(),
197 size_t num_poses =
poses_.size();
199 manual_object_->begin(
"BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST );
200 for(
size_t i=0; i < num_poses; ++i )
202 const Ogre::Vector3 & pos =
poses_[i].position;
203 const Ogre::Quaternion & orient =
poses_[i].orientation;
204 Ogre::Vector3 vertices[6];
206 vertices[1] = pos + orient * Ogre::Vector3( length, 0, 0 );
207 vertices[2] = vertices[ 1 ];
208 vertices[3] = pos + orient * Ogre::Vector3( 0.75*length, 0.2*length, 0 );
209 vertices[4] = vertices[ 1 ];
210 vertices[5] = pos + orient * Ogre::Vector3( 0.75*length, -0.2*length, 0 );
212 for(
int i = 0; i < 6; ++i )
225 case ShapeType::Arrow2d:
230 case ShapeType::Arrow3d:
235 case ShapeType::Axes:
250 Ogre::Quaternion adjust_orientation( Ogre::Degree(-90), Ogre::Vector3::UNIT_Y );
251 for (std::size_t i = 0; i <
poses_.size(); ++i)
254 arrows3d_[i].setOrientation(
poses_[i].orientation * adjust_orientation );
264 for (std::size_t i = 0; i <
poses_.size(); ++i)
313 bool use_arrow2d = shape == ShapeType::Arrow2d;
314 bool use_arrow3d = shape == ShapeType::Arrow3d;
315 bool use_arrow = use_arrow2d || use_arrow3d;
316 bool use_axes = shape == ShapeType::Axes;
341 if (shape == ShapeType::Arrow2d)
345 else if (shape == ShapeType::Arrow3d)
347 for (std::size_t i = 0; i <
arrows3d_.size(); ++i)
363 for (std::size_t i = 0; i <
poses_.size(); ++i)
377 for (std::size_t i = 0; i <
poses_.size(); ++i)
FloatProperty * arrow3d_head_length_property_
void updateAxesGeometry()
Update the axes geometry.
void updateShapeChoice()
Update the interface and visible shapes based on the selected shape type.
Ogre::ColourValue getOgreColor() const
ColorProperty * arrow_color_property_
virtual void onInitialize()
FloatProperty * arrow2d_length_property_
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
bool setTransform(std_msgs::Header const &header)
virtual float getFloat() const
Displays a geometry_msgs/PoseArray message as a bunch of line-drawn arrows.
RosTopicProperty * topic_property_
FloatProperty * axes_radius_property_
void updateArrow2dGeometry()
Update the flat arrow geometry.
Property specialized to enforce floating point max/min.
Ogre::SceneNode * scene_node_
The Ogre::SceneNode to hold all 3D scene elements shown by this Display.
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
FloatProperty * arrow3d_shaft_radius_property_
#define ROS_DEBUG_NAMED(name,...)
std::vector< OgrePose > poses_
FloatProperty * arrow_alpha_property_
virtual ~PoseArrayDisplay()
virtual void onInitialize()
Override this function to do subclass-specific initialization.
virtual void processMessage(const geometry_msgs::PoseArray::ConstPtr &msg)
bool validateFloats(const sensor_msgs::CameraInfo &msg)
virtual void addOption(const QString &option, int value=0)
void updateArrowColor()
Update the arrow color.
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 FrameManager * getFrameManager() const =0
Return the FrameManager instance.
FloatProperty * axes_length_property_
virtual void reset()
Called to tell the display to clear its state.
FloatProperty * arrow3d_shaft_length_property_
float normalizeQuaternion(float &w, float &x, float &y, float &z)
#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().
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.
bool validateQuaternions(const visualization_msgs::InteractiveMarker &marker)
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 * shape_property_
void updateArrow3dGeometry()
Update the 3D arrow geometry.
An arrow consisting of a cylinder and a cone.
boost::ptr_vector< Arrow > arrows3d_
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
Ogre::SceneNode * arrow_node_
bool initialized() const
Returns true if the display has been initialized.
std::string getTopicStd() const
FloatProperty * arrow3d_head_radius_property_
boost::ptr_vector< Axes > axes_
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)
Ogre::ManualObject * manual_object_
Ogre::SceneNode * axes_node_
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.