effort_visual.cpp
Go to the documentation of this file.
00001 #include <OGRE/OgreVector3.h>
00002 #include <OGRE/OgreSceneNode.h>
00003 #include <OGRE/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 <urdf/model.h>
00011 #include "effort_visual.h"
00012 
00013 namespace jsk_rviz_plugin
00014 {
00015 
00016     EffortVisual::EffortVisual( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, boost::shared_ptr<urdf::Model> urdf_model )
00017     {
00018         scene_manager_ = scene_manager;
00019 
00020         // Ogre::SceneNode s form a tree, with each node storing the
00021         // transform (position and orientation) of itself relative to its
00022         // parent.  Ogre does the math of combining those transforms when it
00023         // is time to render.
00024         //
00025         // Here we create a node to store the pose of the Effort's header frame
00026         // relative to the RViz fixed frame.
00027         frame_node_ = parent_node->createChildSceneNode();
00028 
00029         //
00030         urdf_model_ = urdf_model;
00031 
00032         // We create the arrow object within the frame node so that we can
00033         // set its position and direction relative to its header frame.
00034         for (std::map<std::string, boost::shared_ptr<urdf::Joint> >::iterator it = urdf_model_->joints_.begin(); it != urdf_model_->joints_.end(); it ++ ) {
00035             if ( it->second->type == urdf::Joint::REVOLUTE ) {
00036                 std::string joint_name = it->first;
00037                 effort_enabled_[joint_name] = true;
00038             }
00039         }
00040     }
00041 
00042     EffortVisual::~EffortVisual()
00043     {
00044         // Delete the arrow to make it disappear.
00045         for (std::map<std::string, rviz::BillboardLine*>::iterator it = effort_circle_.begin(); it != effort_circle_.end(); it ++ ) {
00046             delete (it->second);
00047         }
00048         for (std::map<std::string, rviz::Arrow*>::iterator it = effort_arrow_.begin(); it != effort_arrow_.end(); it ++ ) {
00049             delete (it->second);
00050         }
00051 
00052         // Destroy the frame node since we don't need it anymore.
00053         scene_manager_->destroySceneNode( frame_node_ );
00054     }
00055 
00056 
00057     void EffortVisual::getRainbowColor(float value, Ogre::ColourValue& color)
00058     {
00059         value = std::min(value, 1.0f);
00060         value = std::max(value, 0.0f);
00061 
00062         float h = value * 5.0f + 1.0f;
00063         int i = floor(h);
00064         float f = h - i;
00065         if ( !(i&1) ) f = 1 - f; // if i is even
00066         float n = 1 - f;
00067 
00068         if      (i <= 1) color[0] = 0, color[1] = n, color[2] = 1;
00069         else if (i == 2) color[0] = 0, color[1] = 1, color[2] = n;
00070         else if (i == 3) color[0] = n, color[1] = 1, color[2] = 0;
00071         else if (i == 4) color[0] = 1, color[1] = n, color[2] = 0;
00072         else if (i >= 5) color[0] = n, color[1] = 0, color[2] = 1;
00073     }
00074 
00075     void EffortVisual::setMessage( const sensor_msgs::JointStateConstPtr& msg )
00076     {
00077         // for all joints...
00078         int joint_num = msg->name.size();
00079         for (int i = 0; i < joint_num; i++ )
00080         {
00081             std::string joint_name = msg->name[i];
00082             double effort = msg->effort[i];
00083             const urdf::Joint* joint = urdf_model_->getJoint(joint_name).get();
00084             int joint_type = joint->type;
00085             if ( joint_type == urdf::Joint::REVOLUTE )
00086             {
00087                 // enable or disable draw
00088                 if ( effort_circle_.find(joint_name) != effort_circle_.end() &&
00089                      !effort_enabled_[joint_name] ) // enable->disable
00090                     {
00091                         delete(effort_circle_[joint_name]);
00092                         delete(effort_arrow_[joint_name]);
00093                         effort_circle_.erase(joint_name);
00094                         effort_arrow_.erase(joint_name);
00095                     }
00096                 if ( effort_circle_.find(joint_name) == effort_circle_.end() &&
00097                      effort_enabled_[joint_name] ) // disable -> enable
00098                     {
00099                         effort_circle_[joint_name] = new rviz::BillboardLine( scene_manager_, frame_node_ );
00100                         effort_arrow_[joint_name] = new rviz::Arrow( scene_manager_, frame_node_ );
00101                     }
00102 
00103                 if ( ! effort_enabled_[joint_name] ) continue;
00104 
00105                 //tf::Transform offset = poseFromJoint(joint);
00106                 boost::shared_ptr<urdf::JointLimits> limit = joint->limits;
00107                 double max_effort = limit->effort, effort_value = 0.05;
00108 
00109                 if ( max_effort != 0.0 )
00110                 {
00111                     effort_value = std::min(fabs(effort) / max_effort, 1.0) + 0.05;
00112                 } else {
00113                     effort_value = fabs(effort) + 0.05;
00114                 }
00115 
00116                 effort_arrow_[joint_name]->set(0, width_*2, width_*2*1.0, width_*2*2.0);
00117                 if ( effort > 0 ) {
00118                     effort_arrow_[joint_name]->setDirection(orientation_[joint_name] * Ogre::Vector3(-1,0,0));
00119                 } else {
00120                     effort_arrow_[joint_name]->setDirection(orientation_[joint_name] * Ogre::Vector3( 1,0,0));
00121                 }
00122                 effort_arrow_[joint_name]->setPosition(orientation_[joint_name] * Ogre::Vector3(0, 0.05+effort_value*scale_*0.5, 0) + position_[joint_name]);
00123                 effort_circle_[joint_name]->clear();
00124                 effort_circle_[joint_name]->setLineWidth(width_);
00125                 for (int i = 0; i < 30; i++) {
00126                     Ogre::Vector3 point = Ogre::Vector3((0.05+effort_value*scale_*0.5)*sin(i*2*M_PI/32), (0.05+effort_value*scale_*0.5)*cos(i*2*M_PI/32), 0);
00127                     if ( effort < 0 ) point.x = -point.x;
00128                     effort_circle_[joint_name]->addPoint(orientation_[joint_name] * point + position_[joint_name]);
00129                 }
00130                 Ogre::ColourValue color;
00131                 getRainbowColor(effort_value, color);
00132                 effort_arrow_[joint_name]->setColor(color.r, color.g, color.b, color.a);
00133                 effort_circle_[joint_name]->setColor(color.r, color.g, color.b, color.a);
00134             }
00135         }
00136     }
00137 
00138     void EffortVisual::setFrameEnabled( const std::string joint_name, const bool e )
00139     {
00140         effort_enabled_[joint_name] = e;
00141     }
00142 
00143     // Position and orientation are passed through to the SceneNode.
00144     void EffortVisual::setFramePosition( const Ogre::Vector3& position )
00145     {
00146         frame_node_->setPosition( position );
00147     }
00148 
00149     void EffortVisual::setFrameOrientation( const Ogre::Quaternion& orientation )
00150     {
00151         frame_node_->setOrientation( orientation );
00152     }
00153     // Position and orientation are passed through to the SceneNode.
00154     void EffortVisual::setFramePosition( const std::string joint_name, const Ogre::Vector3& position )
00155     {
00156         position_[joint_name] = position;
00157     }
00158 
00159     void EffortVisual::setFrameOrientation( const std::string joint_name, const Ogre::Quaternion& orientation )
00160     {
00161         orientation_[joint_name] = orientation;
00162     }
00163 
00164     void EffortVisual::setWidth( float w )
00165     {
00166         width_ = w;
00167     }
00168 
00169     void EffortVisual::setScale( float s )
00170     {
00171         scale_ = s;
00172     }
00173 
00174 } // end namespace jsk_rviz_plugin
00175 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


jsk_rviz_plugins
Author(s): Kei Okada
autogenerated on Sat Mar 23 2013 20:30:29