wrench_visual.cpp
Go to the documentation of this file.
00001 #include <OgreVector3.h>
00002 #include <OgreSceneNode.h>
00003 #include <OgreSceneManager.h>
00004 
00005 #include <rviz/ogre_helpers/arrow.h>
00006 #include <rviz/ogre_helpers/billboard_line.h>
00007 
00008 #include <ros/ros.h>
00009 
00010 #include "wrench_visual.h"
00011 
00012 namespace rviz
00013 {
00014 
00015 WrenchStampedVisual::WrenchStampedVisual( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node )
00016 {
00017     scene_manager_ = scene_manager;
00018 
00019     // Ogre::SceneNode s form a tree, with each node storing the
00020     // transform (position and orientation) of itself relative to its
00021     // parent.  Ogre does the math of combining those transforms when it
00022     // is time to render.
00023     //
00024     // Here we create a node to store the pose of the WrenchStamped's header frame
00025     // relative to the RViz fixed frame.
00026     frame_node_ = parent_node->createChildSceneNode();
00027     force_node_ = frame_node_->createChildSceneNode();
00028     torque_node_ = frame_node_->createChildSceneNode();
00029 
00030     // We create the arrow object within the frame node so that we can
00031     // set its position and direction relative to its header frame.
00032     arrow_force_ = new rviz::Arrow( scene_manager_, force_node_ );
00033     arrow_torque_ = new rviz::Arrow( scene_manager_, torque_node_ );
00034     circle_torque_ = new rviz::BillboardLine( scene_manager_, torque_node_ );
00035     circle_arrow_torque_ = new rviz::Arrow( scene_manager_, torque_node_ );
00036 }
00037 
00038 WrenchStampedVisual::~WrenchStampedVisual()
00039 {
00040     // Delete the arrow to make it disappear.
00041     delete arrow_force_;
00042     delete arrow_torque_;
00043     delete circle_torque_;
00044     delete circle_arrow_torque_;
00045 
00046     // Destroy the frame node since we don't need it anymore.
00047     scene_manager_->destroySceneNode( frame_node_ );
00048 }
00049 
00050 
00051 void WrenchStampedVisual::setMessage( const geometry_msgs::WrenchStamped::ConstPtr& msg )
00052 {
00053     setWrench(msg->wrench);
00054 }
00055 
00056 void WrenchStampedVisual::setWrench( const geometry_msgs::Wrench& wrench )
00057 {
00058     Ogre::Vector3 force(wrench.force.x, wrench.force.y, wrench.force.z);
00059     Ogre::Vector3 torque(wrench.torque.x, wrench.torque.y, wrench.torque.z);
00060     setWrench(force, torque);
00061 }
00062 
00063 void WrenchStampedVisual::setWrench( const Ogre::Vector3 &force, const Ogre::Vector3 &torque )
00064 {
00065     double force_length = force.length() * force_scale_;
00066     double torque_length = torque.length() * torque_scale_;
00067     // hide markers if they get too short
00068     bool show_force = (force_length > width_);
00069     bool show_torque = (torque_length > width_);
00070     if (show_force) {
00071         arrow_force_->setScale(Ogre::Vector3(force_length, width_, width_));
00072         arrow_force_->setDirection(force);
00073     }
00074     force_node_->setVisible(show_force);
00075 
00076     if (show_torque) {
00077         arrow_torque_->setScale(Ogre::Vector3(torque_length, width_, width_));
00078         arrow_torque_->setDirection(torque);
00079         Ogre::Vector3 axis_z(0,0,1);
00080         Ogre::Quaternion orientation = axis_z.getRotationTo(torque);
00081         if ( std::isnan(orientation.x) ||
00082              std::isnan(orientation.y) ||
00083              std::isnan(orientation.z) ) orientation = Ogre::Quaternion::IDENTITY;
00084         //circle_arrow_torque_->setScale(Ogre::Vector3(width_, width_, 0.05));
00085         circle_arrow_torque_->set(0, width_*0.1, width_*0.1*1.0, width_*0.1*2.0);
00086         circle_arrow_torque_->setDirection(orientation * Ogre::Vector3(0,1,0));
00087         circle_arrow_torque_->setPosition(orientation * Ogre::Vector3(torque_length/4, 0, torque_length/2));
00088         circle_torque_->clear();
00089         circle_torque_->setLineWidth(width_*0.05);
00090         for (int i = 4; i <= 32; i++) {
00091             Ogre::Vector3 point = Ogre::Vector3((torque_length/4)*cos(i*2*M_PI/32),
00092                                                 (torque_length/4)*sin(i*2*M_PI/32),
00093                                                 torque_length/2);
00094             circle_torque_->addPoint(orientation * point);
00095         }
00096     }
00097     torque_node_->setVisible(show_torque);
00098 }
00099 
00100 // Position and orientation are passed through to the SceneNode.
00101 void WrenchStampedVisual::setFramePosition( const Ogre::Vector3& position )
00102 {
00103     frame_node_->setPosition( position );
00104 }
00105 
00106 void WrenchStampedVisual::setFrameOrientation( const Ogre::Quaternion& orientation )
00107 {
00108     frame_node_->setOrientation( orientation );
00109 }
00110 
00111 // Color is passed through to the rviz object.
00112 void WrenchStampedVisual::setForceColor( float r, float g, float b, float a )
00113 {
00114     arrow_force_->setColor( r, g, b, a );
00115 }
00116 // Color is passed through to the rviz object.
00117 void WrenchStampedVisual::setTorqueColor( float r, float g, float b, float a )
00118 {
00119     arrow_torque_->setColor( r, g, b, a );
00120     circle_torque_->setColor( r, g, b, a );
00121     circle_arrow_torque_->setColor( r, g, b, a );
00122 }
00123 
00124 void  WrenchStampedVisual::setForceScale( float s )
00125 {
00126     force_scale_ = s;
00127 }
00128 
00129 void  WrenchStampedVisual::setTorqueScale( float s )
00130 {
00131     torque_scale_ = s;
00132 }
00133 
00134 void  WrenchStampedVisual::setWidth( float w )
00135 {
00136     width_ = w;
00137 }
00138 
00139 void WrenchStampedVisual::setVisible(bool visible)
00140 {
00141     frame_node_->setVisible(visible);
00142 }
00143 
00144 } // end namespace rviz


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Tue Oct 3 2017 03:19:31