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
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
00125 }
00126
00127
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
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
00163 void EffortDisplay::updateHistoryLength( )
00164 {
00165 visuals_.rset_capacity(history_length_property_->getInt());
00166 }
00167
00168 void EffortDisplay::load()
00169 {
00170
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, boost::shared_ptr<urdf::Joint> >::iterator it = robot_model_->joints_.begin(); it != robot_model_->joints_.end(); it ++ ) {
00212 boost::shared_ptr<urdf::Joint> joint = it->second;
00213 if ( joint->type == urdf::Joint::REVOLUTE ) {
00214 std::string joint_name = it->first;
00215 boost::shared_ptr<urdf::JointLimits> limit = joint->limits;
00216 joints_[joint_name] = createJoint(joint_name);
00217
00218
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
00268 void EffortDisplay::processMessage( const sensor_msgs::JointState::ConstPtr& msg )
00269 {
00270
00271
00272 boost::shared_ptr<EffortVisual> visual;
00273 if (visuals_.full())
00274 {
00275 visual = visuals_.front();
00276 }
00277 else
00278 {
00279 visual.reset(new EffortVisual(context_->getSceneManager(), scene_node_, robot_model_));
00280 }
00281
00282 V_string joints;
00283 int joint_num = msg->name.size();
00284 if (joint_num != msg->effort.size())
00285 {
00286 std::string tmp_error = "Received a joint state msg with different joint names and efforts size!";
00287 ROS_ERROR("%s", tmp_error.c_str());
00288 setStatus(rviz::StatusProperty::Error, "TOPIC", QString::fromStdString(tmp_error));
00289 return;
00290 }
00291 for (int i = 0; i < joint_num; ++i)
00292 {
00293 std::string joint_name = msg->name[i];
00294 JointInfo* joint_info = getJointInfo(joint_name);
00295 if ( !joint_info ) continue;
00296
00297
00298 joint_info->setEffort(msg->effort[i]);
00299
00300
00301 if ((ros::Time::now() - joint_info->last_update_) > ros::Duration(0.2))
00302 {
00303 joint_info->last_update_ = ros::Time::now();
00304 }
00305
00306 const urdf::Joint* joint = robot_model_->getJoint(joint_name).get();
00307 int joint_type = joint->type;
00308 if ( joint_type == urdf::Joint::REVOLUTE )
00309 {
00310
00311 std::string parent_link_name = joint->child_link_name;
00312 Ogre::Quaternion orientation;
00313 Ogre::Vector3 position;
00314
00315
00316
00317
00318 if( !context_->getFrameManager()->getTransform( parent_link_name,
00319 ros::Time(),
00320
00321 position, orientation ))
00322 {
00323 ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
00324 parent_link_name.c_str(), qPrintable( fixed_frame_) );
00325 continue;
00326 }
00327 ;
00328 tf::Vector3 axis_joint(joint->axis.x, joint->axis.y, joint->axis.z);
00329 tf::Vector3 axis_z(0,0,1);
00330 tf::Quaternion axis_rotation(tf::tfCross(axis_joint, axis_z), tf::tfAngle(axis_joint, axis_z));
00331 if ( std::isnan(axis_rotation.x()) ||
00332 std::isnan(axis_rotation.y()) ||
00333 std::isnan(axis_rotation.z()) ) axis_rotation = tf::Quaternion::getIdentity();
00334
00335 tf::Quaternion axis_orientation(orientation.x, orientation.y, orientation.z, orientation.w);
00336 tf::Quaternion axis_rot = axis_orientation * axis_rotation;
00337 Ogre::Quaternion joint_orientation(Ogre::Real(axis_rot.w()), Ogre::Real(axis_rot.x()), Ogre::Real(axis_rot.y()), Ogre::Real(axis_rot.z()));
00338 visual->setFramePosition( joint_name, position );
00339 visual->setFrameOrientation( joint_name, joint_orientation );
00340 visual->setFrameEnabled( joint_name, joint_info->getEnabled() );
00341 }
00342 }
00343
00344
00345
00346 float scale = scale_property_->getFloat();
00347 float width = width_property_->getFloat();
00348 visual->setWidth( width );
00349 visual->setScale( scale );
00350 visual->setMessage( msg );
00351
00352 visuals_.push_back(visual);
00353 }
00354
00355 }
00356
00357
00358
00359 #include <pluginlib/class_list_macros.h>
00360 PLUGINLIB_EXPORT_CLASS( rviz::EffortDisplay, rviz::Display )
00361
00362