00001 #ifndef WRENCHSTAMPED_VISUAL_H 00002 #define WRENCHSTAMPED_VISUAL_H 00003 00004 #include <geometry_msgs/WrenchStamped.h> 00005 00006 namespace Ogre 00007 { 00008 class Vector3; 00009 class Quaternion; 00010 } 00011 00012 namespace rviz 00013 { 00014 class Arrow; 00015 class BillboardLine; 00016 } 00017 00018 namespace rviz 00019 { 00020 00021 00022 // Each instance of WrenchStampedVisual represents the visualization of a single 00023 // sensor_msgs::WrenchStamped message. Currently it just shows an arrow with 00024 // the direction and magnitude of the acceleration vector, but could 00025 // easily be expanded to include more of the message data. 00026 class WrenchStampedVisual 00027 { 00028 public: 00029 // Constructor. Creates the visual stuff and puts it into the 00030 // scene, but in an unconfigured state. 00031 WrenchStampedVisual( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node ); 00032 00033 // Destructor. Removes the visual stuff from the scene. 00034 virtual ~WrenchStampedVisual(); 00035 00036 // Configure the visual to show the data in the message. 00037 void setMessage( const geometry_msgs::WrenchStamped::ConstPtr& msg ); 00038 00039 // Set the pose of the coordinate frame the message refers to. 00040 // These could be done inside setMessage(), but that would require 00041 // calls to FrameManager and error handling inside setMessage(), 00042 // which doesn't seem as clean. This way WrenchStampedVisual is only 00043 // responsible for visualization. 00044 void setFramePosition( const Ogre::Vector3& position ); 00045 void setFrameOrientation( const Ogre::Quaternion& orientation ); 00046 00047 // Set the color and alpha of the visual, which are user-editable 00048 // parameters and therefore don't come from the WrenchStamped message. 00049 void setForceColor( float r, float g, float b, float a ); 00050 void setTorqueColor( float r, float g, float b, float a ); 00051 void setForceScale( float s ); 00052 void setTorqueScale( float s ); 00053 void setWidth( float w ); 00054 00055 private: 00056 // The object implementing the wrenchStamped circle 00057 rviz::Arrow* arrow_force_; 00058 rviz::Arrow* arrow_torque_; 00059 rviz::BillboardLine* circle_torque_; 00060 rviz::Arrow* circle_arrow_torque_; 00061 float force_scale_, torque_scale_, width_; 00062 00063 // A SceneNode whose pose is set to match the coordinate frame of 00064 // the WrenchStamped message header. 00065 Ogre::SceneNode* frame_node_; 00066 00067 // The SceneManager, kept here only so the destructor can ask it to 00068 // destroy the ``frame_node_``. 00069 Ogre::SceneManager* scene_manager_; 00070 00071 }; 00072 00073 } // end namespace rviz 00074 00075 #endif // WRENCHSTAMPED_VISUAL_H