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 <urdf/model.h>
00011 #include "effort_visual.h"
00012
00013 namespace rviz
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
00021
00022
00023
00024
00025
00026
00027 frame_node_ = parent_node->createChildSceneNode();
00028
00029
00030 urdf_model_ = urdf_model;
00031
00032
00033
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
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
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;
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
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
00088 if ( effort_circle_.find(joint_name) != effort_circle_.end() &&
00089 !effort_enabled_[joint_name] )
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] )
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
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
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
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 }
00175