30 #include <OgreSceneNode.h> 31 #include <OgreSceneManager.h> 49 "Color to draw the range.",
53 "Amount of transparency to apply to the range.",
57 "Number of prior measurements to display.",
62 "Size of the tf message filter queue. It usually needs to be set at least as high as the number of sonar frames.",
75 for(
size_t i = 0; i <
cones_.size(); i++ )
96 for(
size_t i = 0; i <
cones_.size(); i++ )
98 cones_[i]->setColor( oc.r, oc.g, oc.b, alpha );
108 for(
size_t i = 0; i <
cones_.size(); i++ )
112 cones_.resize( buffer_length );
113 for(
size_t i = 0; i <
cones_.size(); i++ )
118 Ogre::Vector3 position;
119 Ogre::Quaternion orientation;
120 geometry_msgs::Pose pose;
121 pose.orientation.w = 1;
122 Ogre::Vector3 scale( 0, 0, 0 );
124 cone->
setColor( color.redF(), color.greenF(), color.blueF(), 0 );
132 Ogre::Vector3 position;
133 Ogre::Quaternion orientation;
134 geometry_msgs::Pose pose;
135 float displayed_range = 0.0;
136 if(msg->min_range <= msg->range && msg->range <= msg->max_range){
137 displayed_range = msg->range;
138 }
else if(msg->min_range == msg->max_range){
139 if(msg->range < 0 && !std::isfinite(msg->range)){
140 displayed_range = msg->min_range;
144 pose.position.x = displayed_range/2 - .008824 * displayed_range;
145 pose.orientation.z = 0.707;
146 pose.orientation.w = 0.707;
149 ROS_DEBUG(
"Error transforming from frame '%s' to frame '%s'",
150 msg->header.frame_id.c_str(), qPrintable(
fixed_frame_ ));
156 double cone_width = 2.0 * displayed_range * tan( msg->field_of_view / 2.0 );
157 Ogre::Vector3 scale( cone_width, displayed_range, cone_width );
std::vector< Shape * > cones_
Handles actually drawing the cones.
virtual QColor getColor() const
ColorProperty * color_property_
virtual void setOrientation(const Ogre::Quaternion &orientation)
Set the orientation of the object.
Ogre::ColourValue getOgreColor() const
virtual void onInitialize()
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
virtual int getInt() const
Return the internal property value as an integer.
virtual float getFloat() const
void updateColorAndAlpha()
tf::MessageFilter< sensor_msgs::Range > * tf_filter_
Property specialized to enforce floating point max/min.
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.
virtual void setColor(float r, float g, float b, float a)
Set the color of the object. Values are in the range [0, 1].
void updateBufferLength()
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
virtual void setQueueSize(uint32_t new_queue_size)
virtual void onInitialize()
Overridden from Display.
IntProperty * queue_size_property_
virtual void setPosition(const Ogre::Vector3 &position)
Set the position of this object.
FloatProperty * alpha_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.
uint32_t messages_received_
virtual Ogre::SceneManager * getSceneManager() const =0
Returns the Ogre::SceneManager used for the main RenderPanel.
virtual void queueRender()=0
Queues a render. Multiple calls before a render happens will only cause a single render.
Displays a sensor_msgs::Range message as a cone.
virtual void processMessage(const sensor_msgs::Range::ConstPtr &msg)
Overridden from MessageFilterDisplay.
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
virtual void setScale(const Ogre::Vector3 &scale)
Set the scale of the object. Always relative to the identity orientation of the object.
IntProperty * buffer_length_property_
virtual void reset()
Overridden from Display.