effort_display.cpp
Go to the documentation of this file.
00001 #include <OgreSceneNode.h>
00002 #include <OgreSceneManager.h>
00003 
00004 #include <tf/transform_listener.h>
00005 
00006 #include <rviz/visualization_manager.h>
00007 #include <rviz/properties/color_property.h>
00008 #include <rviz/properties/float_property.h>
00009 #include <rviz/properties/int_property.h>
00010 #include <rviz/frame_manager.h>
00011 
00012 #include <boost/foreach.hpp>
00013 
00014 #include "effort_visual.h"
00015 
00016 #include "effort_display.h"
00017 
00018 #include <urdf/model.h>
00019 
00020 namespace rviz
00021 {
00022 
00023     JointInfo::JointInfo(const std::string name, rviz::Property* parent_category) {
00024         name_ = name;
00025         effort_ = 0;
00026         max_effort_ = 0;
00027         last_update_ = ros::Time::now();
00028 
00029         //info->category_ = new Property( QString::fromStdString( info->name_ ) , QVariant(), "", joints_category_);
00030         category_ = new rviz::Property( QString::fromStdString( name_ ) , true, "", parent_category, SLOT( updateVisibility() ), this);
00031 
00032         effort_property_ = new rviz::FloatProperty( "Effort", 0, "Effort value of this joint.", category_);
00033         effort_property_->setReadOnly( true );
00034 
00035         max_effort_property_ = new rviz::FloatProperty( "Max Effort", 0, "Max Effort value of this joint.", category_);
00036         max_effort_property_->setReadOnly( true );
00037     }
00038 
00039     JointInfo::~JointInfo() {
00040     }
00041 
00042     void JointInfo::updateVisibility() {
00043         bool enabled = getEnabled();
00044     }
00045 
00046     void JointInfo::setEffort(double e) {
00047         effort_property_->setFloat(e);
00048         effort_ = e;
00049     }
00050     double JointInfo::getEffort() { return effort_; }
00051     void JointInfo::setMaxEffort(double m) {
00052         max_effort_property_->setFloat(m);
00053         max_effort_ = m;
00054     }
00055     double JointInfo::getMaxEffort() { return max_effort_; }
00056 
00057     bool JointInfo::getEnabled() const
00058     {
00059         return category_->getValue().toBool();
00060     }
00061 
00062     JointInfo* EffortDisplay::getJointInfo( const std::string& joint)
00063     {
00064         M_JointInfo::iterator it = joints_.find( joint );
00065         if ( it == joints_.end() )
00066             {
00067                 return NULL;
00068             }
00069 
00070         return it->second;
00071     }
00072 
00073     JointInfo* EffortDisplay::createJoint(const std::string &joint)
00074     {
00075         JointInfo *info = new JointInfo(joint, this);
00076         joints_.insert( std::make_pair( joint, info ) );
00077         return info;
00078     }
00079 
00081     EffortDisplay::EffortDisplay()
00082     {
00083         alpha_property_ =
00084             new rviz::FloatProperty( "Alpha", 1.0,
00085                                      "0 is fully transparent, 1.0 is fully opaque.",
00086                                      this, SLOT( updateColorAndAlpha() ));
00087 
00088         width_property_ =
00089             new rviz::FloatProperty( "Width", 0.02,
00090                                      "Width to drow effort circle",
00091                                      this, SLOT( updateColorAndAlpha() ));
00092 
00093         scale_property_ =
00094             new rviz::FloatProperty( "Scale", 1.0,
00095                                      "Scale to drow effort circle",
00096                                      this, SLOT( updateColorAndAlpha() ));
00097 
00098         history_length_property_ =
00099             new rviz::IntProperty( "History Length", 1,
00100                                    "Number of prior measurements to display.",
00101                                    this, SLOT( updateHistoryLength() ));
00102         history_length_property_->setMin( 1 );
00103         history_length_property_->setMax( 100000 );
00104 
00105 
00106         robot_description_property_ =
00107             new rviz::StringProperty( "Robot Description", "robot_description",
00108                                       "Name of the parameter to search for to load the robot description.",
00109                                       this, SLOT( updateRobotDescription() ) );
00110 
00111 
00112         joints_category_ =
00113             new rviz::Property("Joints", QVariant(), "", this);
00114     }
00115 
00116     void EffortDisplay::onInitialize()
00117     {
00118         MFDClass::onInitialize();
00119         updateHistoryLength();
00120     }
00121 
00122     EffortDisplay::~EffortDisplay()
00123     {
00124         //delete robot_model_; // sharead pointer
00125     }
00126 
00127     // Clear the visuals by deleting their objects.
00128     void EffortDisplay::reset()
00129     {
00130         MFDClass::reset();
00131         visuals_.clear();
00132     }
00133 
00134     void EffortDisplay::clear()
00135     {
00136         clearStatuses();
00137         robot_description_.clear();
00138     }
00139 
00140     // Set the current color and alpha values for each visual.
00141     void EffortDisplay::updateColorAndAlpha()
00142     {
00143         float width = width_property_->getFloat();
00144         float scale = scale_property_->getFloat();
00145 
00146         for( size_t i = 0; i < visuals_.size(); i++ )
00147             {
00148                 visuals_[ i ]->setWidth( width );
00149                 visuals_[ i ]->setScale( scale );
00150             }
00151     }
00152 
00153     void EffortDisplay::updateRobotDescription()
00154     {
00155         if( isEnabled() )
00156             {
00157                 load();
00158                 context_->queueRender();
00159             }
00160     }
00161 
00162     // Set the number of past visuals to show.
00163     void EffortDisplay::updateHistoryLength( )
00164     {
00165         visuals_.rset_capacity(history_length_property_->getInt());
00166     }
00167 
00168     void EffortDisplay::load()
00169     {
00170         // get robot_description
00171         std::string content;
00172         if (!update_nh_.getParam( robot_description_property_->getStdString(), content) )  {
00173             std::string loc;
00174             if( update_nh_.searchParam( robot_description_property_->getStdString(), loc ))
00175                 {
00176                     update_nh_.getParam( loc, content );
00177                 }
00178             else
00179                 {
00180                     clear();
00181                     setStatus( rviz::StatusProperty::Error, "URDF",
00182                                "Parameter [" + robot_description_property_->getString()
00183                                + "] does not exist, and was not found by searchParam()" );
00184                     return;
00185                 }
00186             }
00187 
00188         if( content.empty() )
00189             {
00190                 clear();
00191                 setStatus( rviz::StatusProperty::Error, "URDF", "URDF is empty" );
00192                 return;
00193             }
00194 
00195         if( content == robot_description_ )
00196             {
00197                 return;
00198             }
00199 
00200         robot_description_ = content;
00201 
00202 
00203         robot_model_ = boost::shared_ptr<urdf::Model>(new urdf::Model());
00204         if (!robot_model_->initString(content))
00205         {
00206             ROS_ERROR("Unable to parse URDF description!");
00207             setStatus( rviz::StatusProperty::Error, "URDF", "Unable to parse robot model description!");
00208             return;
00209         }
00210         setStatus(rviz::StatusProperty::Ok, "URDF", "Robot model parserd Ok");
00211     for (std::map<std::string, urdf::JointSharedPtr >::iterator it = robot_model_->joints_.begin(); it != robot_model_->joints_.end(); it ++ ) {
00212         urdf::JointSharedPtr joint = it->second;
00213             if ( joint->type == urdf::Joint::REVOLUTE ) {
00214                 std::string joint_name = it->first;
00215                 urdf::JointLimitsSharedPtr limit = joint->limits;
00216                 joints_[joint_name] = createJoint(joint_name);
00217                 //joints_[joint_name]->max_effort_property_->setFloat(limit->effort);
00218                 //joints_[joint_name]->max_effort_property_->setReadOnly( true );
00219                 joints_[joint_name]->setMaxEffort(limit->effort);
00220             }
00221         }
00222     }
00223 
00224     void EffortDisplay::onEnable()
00225     {
00226         load();
00227     }
00228 
00229     void EffortDisplay::onDisable()
00230     {
00231         clear();
00232     }
00233 
00234 #if 0
00235     void EffortDisplay::setRobotDescription( const std::string& description_param )
00236     {
00237         description_param_ = description_param;
00238 
00239         propertyChanged(robot_description_property_);
00240 
00241         if ( isEnabled() )
00242             {
00243                 load();
00244                 unsubscribe();
00245                 subscribe();
00246                 causeRender();
00247             }
00248     }
00249 
00250     void EffortDisplay::setAllEnabled(bool enabled)
00251     {
00252         all_enabled_ = enabled;
00253 
00254         M_JointInfo::iterator it = joints_.begin();
00255         M_JointInfo::iterator end = joints_.end();
00256         for (; it != end; ++it)
00257             {
00258                 JointInfo* joint = it->second;
00259 
00260                 setJointEnabled(joint, enabled);
00261             }
00262 
00263         propertyChanged(all_enabled_property_);
00264     }
00265 
00266 #endif
00267     // This is our callback to handle an incoming message.
00268     void EffortDisplay::processMessage( const sensor_msgs::JointState::ConstPtr& msg )
00269     {
00270         // Robot model might not be loaded already
00271         if (!robot_model_)
00272         {
00273             return;
00274         }
00275         // We are keeping a circular buffer of visual pointers.  This gets
00276         // the next one, or creates and stores it if the buffer is not full
00277         boost::shared_ptr<EffortVisual> visual;
00278         if (visuals_.full())
00279         {
00280             visual = visuals_.front();
00281         }
00282         else
00283         {
00284             visual.reset(new EffortVisual(context_->getSceneManager(), scene_node_, robot_model_));
00285         }
00286 
00287         V_string joints;
00288         int joint_num = msg->name.size();
00289         if (joint_num != msg->effort.size())
00290         {
00291             std::string tmp_error = "Received a joint state msg with different joint names and efforts size!";
00292             ROS_ERROR("%s", tmp_error.c_str());
00293             setStatus(rviz::StatusProperty::Error, "TOPIC", QString::fromStdString(tmp_error));
00294             return;
00295         }
00296         for (int i = 0; i < joint_num; ++i)
00297         {
00298             std::string joint_name = msg->name[i];
00299             JointInfo* joint_info = getJointInfo(joint_name);
00300             if ( !joint_info ) continue; // skip joints..
00301 
00302             // set effort
00303             joint_info->setEffort(msg->effort[i]);
00304 
00305             // update effort property ???
00306             if ((ros::Time::now() - joint_info->last_update_) > ros::Duration(0.2))
00307             {
00308                 joint_info->last_update_ = ros::Time::now();
00309             }
00310 
00311             const urdf::Joint* joint = robot_model_->getJoint(joint_name).get();
00312             int joint_type = joint->type;
00313             if ( joint_type == urdf::Joint::REVOLUTE )
00314             {
00315                 // we expects that parent_link_name equals to frame_id.
00316                 std::string parent_link_name = joint->child_link_name;
00317                 Ogre::Quaternion orientation;
00318                 Ogre::Vector3 position;
00319 
00320                 // Here we call the rviz::FrameManager to get the transform from the
00321                 // fixed frame to the frame in the header of this Effort message.  If
00322                 // it fails, we can't do anything else so we return.
00323                 if( !context_->getFrameManager()->getTransform( parent_link_name,
00324                                                                 ros::Time(),
00325                                                                 //msg->header.stamp, // ???
00326                                                                 position, orientation ))
00327                 {
00328                     ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
00329                                parent_link_name.c_str(), qPrintable( fixed_frame_) );
00330                     continue;
00331                 }
00332 ;
00333                 tf::Vector3 axis_joint(joint->axis.x, joint->axis.y, joint->axis.z);
00334                 tf::Vector3 axis_z(0,0,1);
00335                 tf::Quaternion axis_rotation(tf::tfCross(axis_joint, axis_z), tf::tfAngle(axis_joint, axis_z));
00336                 if ( std::isnan(axis_rotation.x()) ||
00337                      std::isnan(axis_rotation.y()) ||
00338                      std::isnan(axis_rotation.z()) ) axis_rotation = tf::Quaternion::getIdentity();
00339 
00340                 tf::Quaternion axis_orientation(orientation.x, orientation.y, orientation.z, orientation.w);
00341                 tf::Quaternion axis_rot = axis_orientation * axis_rotation;
00342                 Ogre::Quaternion joint_orientation(Ogre::Real(axis_rot.w()), Ogre::Real(axis_rot.x()), Ogre::Real(axis_rot.y()), Ogre::Real(axis_rot.z()));
00343                 visual->setFramePosition( joint_name, position );
00344                 visual->setFrameOrientation( joint_name, joint_orientation );
00345                 visual->setFrameEnabled( joint_name, joint_info->getEnabled() );
00346             }
00347         }
00348 
00349 
00350         // Now set or update the contents of the chosen visual.
00351         float scale = scale_property_->getFloat();
00352         float width = width_property_->getFloat();
00353         visual->setWidth( width );
00354         visual->setScale( scale );
00355         visual->setMessage( msg );
00356 
00357         visuals_.push_back(visual);
00358     }
00359 
00360 } // end namespace rviz
00361 
00362 // Tell pluginlib about this class.  It is important to do this in
00363 // global scope, outside our package's namespace.
00364 #include <pluginlib/class_list_macros.h>
00365 PLUGINLIB_EXPORT_CLASS( rviz::EffortDisplay, rviz::Display )
00366 
00367 


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Jun 6 2019 18:02:15