00001 #include <boost/format.hpp>
00002 #include "articulation_model_display.h"
00003 #include "rviz/common.h"
00004 #include "rviz/visualization_manager.h"
00005 #include "rviz/properties/property.h"
00006 #include "rviz/properties/property_manager.h"
00007
00008 #include <ogre_tools/axes.h>
00009
00010 #include <tf/transform_listener.h>
00011 #include <OGRE/OgreSceneNode.h>
00012 #include <OGRE/OgreSceneManager.h>
00013
00014 #include "utils.h"
00015 #include "articulation_models/utils.h"
00016
00017 using namespace std;
00018
00019 namespace articulation_rviz_plugin {
00020
00021 ModelDisplay::ModelDisplay(const std::string& name,
00022 rviz::VisualizationManager* manager) :
00023 Display(name, manager), model_topic_("/model"), unique_(u_track),color_(0.5f, 0.0f, 1.0f),
00024 alpha_(0.5f), lineWidth_(0.003f), trackColor_(cs_fixed),modelColor_(cs_fixed),
00025 poseColor_(cs_fixed), modelTypeColor_(cs_fixed), likelihoodColor_(cs_fixed), forceColor_(cs_fixed), displayStyle_(ds_line),
00026 sampleConfigurations_(true), projectConfigurations_(true),
00027 showJacobians_(false), showHessians_(false), showLatestOnly_(false),
00028 sampleDensity_(0.01), showModelName_(false),showForces_(false),forceScaling_(0.01),tf_filter_(*manager->getTFClient(), "",
00029 1000, update_nh_) {
00030 scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00031
00032 tf_filter_.connectInput(sub_);
00033 tf_filter_.registerCallback(boost::bind(&ModelDisplay::incomingModel, this,
00034 _1));
00035 }
00036
00037 ModelDisplay::~ModelDisplay() {
00038 unsubscribe();
00039
00040 }
00041
00042 void ModelDisplay::clearMap() {
00043 std::map<int, std::vector<ogre_tools::BillboardLine*> >::iterator it;
00044 for (it = lines.begin(); it != lines.end(); it++) {
00045 clearVector(it->second);
00046 }
00047 std::map<int, std::vector<ogre_tools::Arrow*> >::iterator it2;
00048 for (it2 = arrows.begin(); it2 != arrows.end(); it2++) {
00049 clearVector(it2->second);
00050 }
00051 std::map<int, std::vector<ogre_tools::MovableText*> >::iterator it3;
00052 for (it3 = text.begin(); it3 != text.end(); it3++) {
00053 clearVector(it3->second);
00054 }
00055 }
00056
00057 void ModelDisplay::clearVector(std::vector<ogre_tools::BillboardLine*>& vec) {
00058 for (size_t j = 0; j < vec.size(); j++) {
00059 delete vec[j];
00060 }
00061 vec.clear();
00062 }
00063
00064 void ModelDisplay::clearVector(std::vector<ogre_tools::Arrow*>& vec) {
00065 for (size_t j = 0; j < vec.size(); j++) {
00066 delete vec[j];
00067 }
00068 vec.clear();
00069 }
00070
00071 void ModelDisplay::clearVector(std::vector<ogre_tools::MovableText*>& vec) {
00072 for (size_t j = 0; j < vec.size(); j++) {
00073 delete vec[j];
00074 }
00075 vec.clear();
00076 }
00077
00078 void ModelDisplay::setTopic(const std::string& topic) {
00079 unsubscribe();
00080 model_topic_ = topic;
00081 subscribe();
00082
00083 propertyChanged(topic_property_);
00084 }
00085
00086 void ModelDisplay::onEnable() {
00087 subscribe();
00088
00089 scene_node_->setVisible(true);
00090 }
00091
00092 void ModelDisplay::onDisable() {
00093 unsubscribe();
00094 scene_node_->setVisible(false);
00095 clearDisplay();
00096 }
00097
00098 void ModelDisplay::reset() {
00099 clearDisplay();
00100 }
00101
00102 void ModelDisplay::subscribe() {
00103 if (!isEnabled()) {
00104 return;
00105 }
00106
00107 if (!model_topic_.empty()) {
00108 sub_.subscribe(update_nh_, model_topic_, 0);
00109 }
00110
00111 }
00112
00113 void ModelDisplay::unsubscribe() {
00114 sub_.unsubscribe();
00115 }
00116
00117 void ModelDisplay::incomingModel(
00118 const articulation_msgs::ModelMsg::ConstPtr& msg) {
00119 incoming_model_message_ = msg;
00120 boost::mutex::scoped_lock lock(queue_mutex_);
00121
00122 message_queue_.push_back(msg);
00123 }
00124
00125 void ModelDisplay::clearResources(int uniqueId) {
00126 for (size_t l = 0; l < lines[uniqueId].size(); l++)
00127 lines[uniqueId][l]->clear();
00128 recycleLines.insert(recycleLines.begin(),
00129 lines[uniqueId].begin(),
00130 lines[uniqueId].end());
00131 lines[uniqueId].clear();
00132
00133 for (size_t l = 0; l < arrows[uniqueId].size(); l++)
00134 arrows[uniqueId][l]->set(0,0,0,0);
00135 recycleArrows.insert(recycleArrows.begin(),
00136 arrows[uniqueId].begin(),
00137 arrows[uniqueId].end());
00138 arrows[uniqueId].clear();
00139
00140 for (size_t l = 0; l < text[uniqueId].size(); l++) {
00141 text[uniqueId][l]->setCaption("-");
00142 text[uniqueId][l]->setVisible(false);
00143 }
00144 recycleText.insert(recycleText.begin(),
00145 text[uniqueId].begin(),
00146 text[uniqueId].end());
00147 text[uniqueId].clear();
00148 }
00149
00150 void ModelDisplay::update(float wall_dt, float ros_dt) {
00151 V_ModelMsg local_queue;
00152
00153 {
00154 boost::mutex::scoped_lock lock(queue_mutex_);
00155
00156 local_queue.swap(message_queue_);
00157 }
00158
00159 if(unique_ == u_modelstored) clearResources(-1);
00160
00161 for (size_t t = 0; t < local_queue.size(); t++) {
00162 const articulation_msgs::ModelMsg::ConstPtr& model_message_orig =
00163 local_queue[t];
00164
00165 int uniqueId;
00166 static int uniqueCounter=0;
00167 switch(unique_) {
00168 case u_all: uniqueId=uniqueCounter++;break;
00169 case u_single: uniqueId=0;break;
00170 case u_track: uniqueId=model_message_orig->track.id;break;
00171 case u_model: uniqueId=model_message_orig->id;break;
00172 case u_modelstored: uniqueId=model_message_orig->id;break;
00173 case u_modeltype: uniqueId=factory.getModelIndex(model_message_orig->name); break;
00174 }
00175
00176 clearResources(uniqueId);
00177
00178 btTransform framePose;
00179 btVector3 lineScale(lineWidth_, lineWidth_, lineWidth_);
00180 transform(model_message_orig, framePose);
00181
00182
00183 articulation_models::GenericModelPtr model = factory.restoreModel(model_message_orig);
00184 if(!model) continue;
00185
00186 articulation_msgs::ModelMsg model_message = model->getModel();
00187
00188 int channel_w = articulation_models::openChannel(model_message.track,"width",false);
00189 int channel_h = articulation_models::openChannel(model_message.track,"height",false);
00190 int channel_logli = articulation_models::openChannel(model_message.track,"loglikelihood",false);
00191 int force_x = articulation_models::openChannel(model_message.track,"force.x",false);
00192 int force_y = articulation_models::openChannel(model_message.track,"force.y",false);
00193 int force_z = articulation_models::openChannel(model_message.track,"force.z",false);
00194 int force_mag = articulation_models::openChannel(model_message.track,"force",false);
00195
00196 float avg_w = lineWidth_*5;
00197 float avg_h = lineWidth_*5;
00198 if(channel_w!=-1) {
00199 for(size_t i=0;i<model_message.track.channels[channel_w].values.size();i++) {
00200 avg_w +=model_message.track.channels[channel_w].values[i];
00201 }
00202 avg_w /= model_message.track.channels[channel_w].values.size();
00203 }
00204 if(channel_h!=-1) {
00205 for(size_t i=0;i<model_message.track.channels[channel_w].values.size();i++) {
00206 avg_h +=model_message.track.channels[channel_h].values[i];
00207 }
00208 avg_h /= model_message.track.channels[channel_w].values.size();
00209 }
00210
00211 Ogre::Vector3 old_pos(0, 0, 0);
00212
00213 std::vector<geometry_msgs::Pose> pose;
00214 if(sampleConfigurations_)
00215 pose = model_message.track.pose_resampled;
00216 else if(projectConfigurations_)
00217 pose = model_message.track.pose_projected;
00218 else
00219 pose = model_message.track.pose;
00220
00221 btTransform rectangle_pose(btQuaternion(0,0,0,1),btVector3(0,0,0));
00222 Ogre::Vector3 pos, scale;
00223 btVector3 color;
00224 Ogre::Quaternion orient;
00225 bool isConnected = false;
00226 size_t start_from=0;
00227 if (pose.size()>0 && showLatestOnly_) start_from = pose.size()-1;
00228 for (size_t i = start_from; i < pose.size(); i++) {
00229 if(!sampleConfigurations_ &&
00230 (model_message.track.pose_flags[i] & articulation_msgs::TrackMsg::POSE_VISIBLE)==0 ) {
00231 isConnected = false;
00232 continue;
00233 }
00234 const geometry_msgs::Pose& geo_pose = pose[i];
00235
00236 rectangle_pose = btTransform(btQuaternion(geo_pose.orientation.x,
00237 geo_pose.orientation.y, geo_pose.orientation.z,
00238 geo_pose.orientation.w), btVector3(geo_pose.position.x,
00239 geo_pose.position.y, geo_pose.position.z));
00240
00241 transform(framePose * rectangle_pose, lineScale, pos, orient, scale);
00242
00243 color = btVector3(color_.r_, color_.g_, color_.b_);
00244
00245 double f = model_message.track.id / 11.0;
00246
00247 color = modifyColor(color, trackColor_, f - floor(f));
00248
00249 double f2 = model_message.id / 11.0;
00250
00251 color = modifyColor(color, modelColor_, f2 - floor(f2));
00252
00253 color = modifyColor(color, poseColor_, i
00254 / (double) pose.size());
00255
00256 color = modifyColor(color, modelTypeColor_,
00257 factory.getModelIndex(model_message.name)/(double)factory.getFactoryCount());
00258
00259 if(channel_logli >=0 && !sampleConfigurations_) {
00260 double logli = model_message.track.channels[ channel_logli ].values[i];
00261 double chi2_70 = 2.41;
00262 double chi2_95 = 5.99;
00263 double chi2_99 = 9.21;
00264 double chi2_999 = 13.816;
00265
00266 double loglikelihood_ellipse_70=
00267 - log(2*M_PI * model->sigma_position*model->sigma_orientation)
00268 - 0.5*( chi2_70 );
00269 double loglikelihood_ellipse_95 =
00270 - log(2*M_PI * model->sigma_position*model->sigma_orientation)
00271 - 0.5*( chi2_95 );
00272 double loglikelihood_ellipse_99 =
00273 - log(2*M_PI * model->sigma_position*model->sigma_orientation)
00274 - 0.5*( chi2_99 );
00275 double loglikelihood_ellipse_999 =
00276 - log(2*M_PI * model->sigma_position*model->sigma_orientation)
00277 - 0.5*( chi2_999 );
00278
00279 double ratio = (logli - loglikelihood_ellipse_70)/(loglikelihood_ellipse_99 - loglikelihood_ellipse_70);
00280
00281
00282
00283
00284
00285
00286 double col = max(0.00,min(1.00,1-ratio));
00287 if(likelihoodColor_ == cs_hue) {
00288 col /= 3;
00289 }
00290 color = modifyColor(color, likelihoodColor_, col);
00291 }
00292
00293 if (force_mag!=-1) {
00294 double force = model_message.track.channels[force_mag].values[i];
00295 if(forceColor_ == cs_hue)
00296 color = modifyColor(color, forceColor_, 1.00/3.00 - max(0.00,min(1.00,force/30.0))/3);
00297 else
00298 color = modifyColor(color, forceColor_, max(0.00,min(1.00,force/30.0)));
00299 } else
00300 if(force_x!=-1 && force_y!=-1 && force_z!=-1) {
00301 double force = sqrt(
00302 SQR(model_message.track.channels[force_x].values[i]) +
00303 SQR(model_message.track.channels[force_y].values[i]) +
00304 SQR(model_message.track.channels[force_z].values[i]) );
00305 if(forceColor_ == cs_hue)
00306 color = modifyColor(color, forceColor_, 1.00/3.00 - max(0.00,min(1.00,force/30.0))/3);
00307 else
00308 color = modifyColor(color, forceColor_, max(0.00,min(1.00,force/30.0)));
00309 }
00310
00311 if (displayStyle_ == ds_line) {
00312 if (isConnected)
00313 createLine(pos, old_pos, scale, color, lines[uniqueId],false,true);
00314 old_pos = pos;
00315 } else if(displayStyle_ == ds_cross_line) {
00316 if (isConnected)
00317 createLine(pos, old_pos, scale, color, lines[uniqueId],true,true);
00318 old_pos = pos;
00319 } else if(displayStyle_ == ds_cross) {
00320 if (isConnected)
00321 createLine(pos, old_pos, scale, color, lines[uniqueId],true,false);
00322 old_pos = pos;
00323 } else if(displayStyle_ == ds_arrow) {
00324 if (isConnected)
00325 createArrow(pos, old_pos, scale, color, arrows[uniqueId]);
00326 old_pos = pos;
00327 } else if (displayStyle_ == ds_axes) {
00328 createAxes(pos, orient, scale, color, lines[uniqueId]);
00329
00330 } else if (displayStyle_ == ds_rectangle) {
00331 float w = avg_w;
00332 float h = avg_h;
00333 if(channel_w != -1) {
00334 if(!sampleConfigurations_ && !projectConfigurations_) {
00335 w=model_message.track.channels[channel_w].values[i];
00336 h=model_message.track.channels[channel_h].values[i];
00337 }
00338 }
00339
00340
00341 createRectangle(
00342 pos,
00343 orient,
00344 scale,
00345 w,h,
00346 color, lines[uniqueId]);
00347 }
00348 if(sampleConfigurations_)
00349 isConnected = true;
00350 else
00351 isConnected = (model_message.track.pose_flags[i] & articulation_msgs::TrackMsg::POSE_END_OF_SEGMENT)==0;
00352
00353 if(showForces_ && force_x!=-1 && force_y!=-1 && force_z!=-1) {
00354 Ogre::Vector3 pos2, scale2;
00355 rectangle_pose = btTransform(
00356 btQuaternion(
00357 geo_pose.orientation.x,
00358 geo_pose.orientation.y,
00359 geo_pose.orientation.z,
00360 geo_pose.orientation.w),
00361 btVector3(
00362 geo_pose.position.x + model_message.track.channels[force_x].values[i]*forceScaling_,
00363 geo_pose.position.y + model_message.track.channels[force_y].values[i]*forceScaling_,
00364 geo_pose.position.z + model_message.track.channels[force_z].values[i]*forceScaling_));
00365
00366 transform(framePose * rectangle_pose, lineScale, pos2, orient, scale2);
00367
00368 createArrow(pos2, pos, scale2, color, arrows[uniqueId]);
00369 }
00370
00371 }
00372
00373
00374 if(showJacobians_ && pose.size()>0 && model->getDOFs()>0) {
00375 geometry_msgs::Pose geo_pose;
00376 if(projectConfigurations_)
00377 geo_pose=model_message.track.pose_projected.back();
00378 else
00379 geo_pose=model_message.track.pose.back();
00380
00381 rectangle_pose = btTransform(btQuaternion(geo_pose.orientation.x,
00382 geo_pose.orientation.y, geo_pose.orientation.z,
00383 geo_pose.orientation.w), btVector3(geo_pose.position.x,
00384 geo_pose.position.y, geo_pose.position.z));
00385 transform(framePose * rectangle_pose, lineScale, pos, orient, scale);
00386
00387 Eigen::MatrixXd J(3,model->getDOFs());
00388 model->getParam("jacobian",J);
00389
00390
00391 scale *= 5;
00392
00393 for(int r=0;r<J.cols();r++) {
00394 btVector3 dir(J(0,r),J(1,r),J(2,r));
00395 dir.normalize();
00396 dir *= 20*lineScale;
00397
00398 btVector3 endpoint = rectangle_pose.getOrigin() + dir;
00399 Ogre::Vector3 pos2 = Ogre::Vector3(endpoint.x(), endpoint.y(), endpoint.z());
00400 rviz::robotToOgre(pos2);
00401
00402 createArrow(pos2, pos, scale, color, arrows[uniqueId]);
00403 }
00404
00405 }
00406
00407
00408 if(showHessians_ && pose.size()>0 && model->getDOFs()>0) {
00409 geometry_msgs::Pose geo_pose;
00410 if(projectConfigurations_)
00411 geo_pose=model_message.track.pose_projected.back();
00412 else
00413 geo_pose=model_message.track.pose.back();
00414
00415 rectangle_pose = btTransform(btQuaternion(geo_pose.orientation.x,
00416 geo_pose.orientation.y, geo_pose.orientation.z,
00417 geo_pose.orientation.w), btVector3(geo_pose.position.x,
00418 geo_pose.position.y, geo_pose.position.z));
00419 transform(framePose * rectangle_pose, lineScale, pos, orient, scale);
00420
00421 Eigen::MatrixXd H((int)(3*model->getDOFs()),(int)(model->getDOFs()));
00422 model->getParam("hessian",H);
00423
00424
00425 scale *= 5;
00426
00427 for(int i=0;i<H.rows()/3;i++)
00428 for(int c=0;c<H.cols();c++) {
00429 btVector3 dir(H(0+3*i,c),H(1+3*i,c),H(2+3*i,c));
00430
00431 if(dir.length()==0) continue;
00432 dir.normalize();
00433 dir *= 10*lineScale;
00434
00435 btVector3 endpoint = rectangle_pose.getOrigin() + dir;
00436 Ogre::Vector3 pos2 = Ogre::Vector3(endpoint.x(), endpoint.y(), endpoint.z());
00437 rviz::robotToOgre(pos2);
00438
00439 createArrow(pos2, pos, scale, color, arrows[uniqueId]);
00440 }
00441
00442 }
00443
00444
00445
00446 if(showModelName_)
00447 {
00448
00449 string caption = boost::str( boost::format("%1%") % model_message.name );
00450
00451 const geometry_msgs::Pose& geo_pose = pose.back();
00452 rectangle_pose = btTransform(btQuaternion(geo_pose.orientation.x,
00453 geo_pose.orientation.y, geo_pose.orientation.z,
00454 geo_pose.orientation.w), btVector3(geo_pose.position.x,
00455 geo_pose.position.y, geo_pose.position.z));
00456
00457 transform(framePose * rectangle_pose, lineScale, pos, orient, scale);
00458
00459 createText(pos,orient, scale, color,caption, text[uniqueId]);
00460 }
00461
00462 }
00463 }
00464
00465 void ModelDisplay::targetFrameChanged() {
00466 }
00467
00468 void ModelDisplay::fixedFrameChanged() {
00469 tf_filter_.setTargetFrame(fixed_frame_);
00470
00471 clearDisplay();
00472 }
00473
00474 void ModelDisplay::createProperties() {
00475 topic_property_ = property_manager_->createProperty<
00476 rviz::ROSTopicStringProperty> ("Topic", property_prefix_,
00477 boost::bind(&ModelDisplay::getTopic, this), boost::bind(
00478 &ModelDisplay::setTopic, this, _1), parent_category_, this);
00479
00480 rviz::ROSTopicStringPropertyPtr topic_prop = topic_property_.lock();
00481 topic_prop->setMessageType(articulation_msgs::ModelMsg::__s_getDataType());
00482
00483 unique_property_
00484 = property_manager_->createProperty<rviz::EnumProperty> (
00485 "Unique by", property_prefix_, boost::bind(
00486 &ModelDisplay::getUnique, this), boost::bind(
00487 &ModelDisplay::setUnique, this, _1),
00488 parent_category_, this);
00489 rviz::EnumPropertyPtr enum_prop1a = unique_property_.lock();
00490 enum_prop1a->addOption("all", u_all);
00491 enum_prop1a->addOption("single", u_single);
00492 enum_prop1a->addOption("track id", u_track);
00493 enum_prop1a->addOption("model id", u_model);
00494 enum_prop1a->addOption("model id (stored only)", u_modelstored);
00495 enum_prop1a->addOption("model type", u_modeltype);
00496
00497 color_property_ = property_manager_->createProperty<rviz::ColorProperty> (
00498 "Color", property_prefix_, boost::bind(&ModelDisplay::getColor,
00499 this), boost::bind(&ModelDisplay::setColor, this, _1),
00500 parent_category_, this);
00501
00502 alpha_property_ = property_manager_->createProperty<rviz::FloatProperty> (
00503 "Alpha", property_prefix_, boost::bind(&ModelDisplay::getAlpha,
00504 this), boost::bind(&ModelDisplay::setAlpha, this, _1),
00505 parent_category_, this);
00506
00507 lineWidth_property_
00508 = property_manager_->createProperty<rviz::FloatProperty> (
00509 "Line Width", property_prefix_, boost::bind(
00510 &ModelDisplay::getLineWidth, this), boost::bind(
00511 &ModelDisplay::setLineWidth, this, _1),
00512 parent_category_, this);
00513
00514 trackColor_property_
00515 = property_manager_->createProperty<rviz::EnumProperty> (
00516 "Track number", property_prefix_, boost::bind(
00517 &ModelDisplay::getTrackColor, this), boost::bind(
00518 &ModelDisplay::setTrackColor, this, _1),
00519 parent_category_, this);
00520 rviz::EnumPropertyPtr enum_prop1 = trackColor_property_.lock();
00521 enum_prop1->addOption("not used", cs_fixed);
00522
00523 enum_prop1->addOption("as hue", cs_hue);
00524 enum_prop1->addOption("as brightness", cs_brightness);
00525
00526 modelColor_property_
00527 = property_manager_->createProperty<rviz::EnumProperty> (
00528 "Model number", property_prefix_, boost::bind(
00529 &ModelDisplay::getModelColor, this), boost::bind(
00530 &ModelDisplay::setModelColor, this, _1),
00531 parent_category_, this);
00532 rviz::EnumPropertyPtr enum_prop11 = modelColor_property_.lock();
00533 enum_prop11->addOption("not used", cs_fixed);
00534
00535 enum_prop11->addOption("as hue", cs_hue);
00536 enum_prop11->addOption("as brightness", cs_brightness);
00537
00538 poseColor_property_
00539 = property_manager_->createProperty<rviz::EnumProperty> (
00540 "Sequence number", property_prefix_, boost::bind(
00541 &ModelDisplay::getPoseColor, this), boost::bind(
00542 &ModelDisplay::setPoseColor, this, _1),
00543 parent_category_, this);
00544 rviz::EnumPropertyPtr enum_prop2 = poseColor_property_.lock();
00545 enum_prop2->addOption("not used", cs_fixed);
00546
00547 enum_prop2->addOption("as hue", cs_hue);
00548 enum_prop2->addOption("as brightness", cs_brightness);
00549
00550 modelTypeColor_property_
00551 = property_manager_->createProperty<rviz::EnumProperty> (
00552 "Model type", property_prefix_, boost::bind(
00553 &ModelDisplay::getModelTypeColor, this), boost::bind(
00554 &ModelDisplay::setModelTypeColor, this, _1),
00555 parent_category_, this);
00556 rviz::EnumPropertyPtr enum_prop22 = modelTypeColor_property_.lock();
00557 enum_prop22->addOption("not used", cs_fixed);
00558 enum_prop22->addOption("as hue", cs_hue);
00559 enum_prop22->addOption("as brightness", cs_brightness);
00560
00561 likelihoodColor_property_
00562 = property_manager_->createProperty<rviz::EnumProperty> (
00563 "Pose Likelihood", property_prefix_, boost::bind(
00564 &ModelDisplay::getLikelihoodColor, this), boost::bind(
00565 &ModelDisplay::setLikelihoodColor, this, _1),
00566 parent_category_, this);
00567 rviz::EnumPropertyPtr enum_prop3 = likelihoodColor_property_.lock();
00568 enum_prop3->addOption("not used", cs_fixed);
00569 enum_prop3->addOption("as hue", cs_hue);
00570 enum_prop3->addOption("as brightness", cs_brightness);
00571
00572 forceColor_property_
00573 = property_manager_->createProperty<rviz::EnumProperty> (
00574 "Force (x/y/z or mag)", property_prefix_, boost::bind(
00575 &ModelDisplay::getForceColor, this), boost::bind(
00576 &ModelDisplay::setForceColor, this, _1),
00577 parent_category_, this);
00578 rviz::EnumPropertyPtr enum_prop3f = forceColor_property_.lock();
00579 enum_prop3f->addOption("not used", cs_fixed);
00580 enum_prop3f->addOption("as hue", cs_hue);
00581 enum_prop3f->addOption("as brightness", cs_brightness);
00582
00583 displayStyle_property_ = property_manager_->createProperty<
00584 rviz::EnumProperty> ("Style", property_prefix_, boost::bind(
00585 &ModelDisplay::getDisplayStyle, this), boost::bind(
00586 &ModelDisplay::setDisplayStyle, this, _1), parent_category_, this);
00587 rviz::EnumPropertyPtr enum_prop4 = displayStyle_property_.lock();
00588 enum_prop4->addOption("line", ds_line);
00589 enum_prop4->addOption("cross line", ds_cross_line);
00590 enum_prop4->addOption("cross", ds_cross);
00591 enum_prop4->addOption("arrow", ds_arrow);
00592 enum_prop4->addOption("axes", ds_axes);
00593 enum_prop4->addOption("rectangle", ds_rectangle);
00594 enum_prop4->addOption("none", ds_none);
00595
00596 sampleConfigurations_property_ = property_manager_->createProperty<
00597 rviz::BoolProperty> ("Sample Configurations", property_prefix_,
00598 boost::bind(&ModelDisplay::getSampleConfigurations, this),
00599 boost::bind(&ModelDisplay::setSampleConfigurations, this, _1),
00600 parent_category_, this);
00601
00602 sampleDensity_property_ = property_manager_->createProperty<
00603 rviz::FloatProperty> ("Sample Density", property_prefix_,
00604 boost::bind(&ModelDisplay::getSampleDensity, this), boost::bind(
00605 &ModelDisplay::setSampleDensity, this, _1),
00606 parent_category_, this);
00607
00608
00609 projectConfigurations_property_ = property_manager_->createProperty<
00610 rviz::BoolProperty> ("Project Configurations", property_prefix_,
00611 boost::bind(&ModelDisplay::getProjectConfigurations, this),
00612 boost::bind(&ModelDisplay::setProjectConfigurations, this, _1),
00613 parent_category_, this);
00614
00615 showJacobians_property_ = property_manager_->createProperty<
00616 rviz::BoolProperty> ("Jacobians", property_prefix_, boost::bind(
00617 &ModelDisplay::getShowJacobians, this), boost::bind(
00618 &ModelDisplay::setShowJacobians, this, _1), parent_category_, this);
00619
00620 showHessians_property_ = property_manager_->createProperty<
00621 rviz::BoolProperty> ("Hessians", property_prefix_, boost::bind(
00622 &ModelDisplay::getShowHessians, this), boost::bind(
00623 &ModelDisplay::setShowHessians, this, _1), parent_category_, this);
00624
00625 showLatestOnly_property_ = property_manager_->createProperty<
00626 rviz::BoolProperty> ("Latest obs only", property_prefix_,
00627 boost::bind(&ModelDisplay::getShowLatestOnly, this), boost::bind(
00628 &ModelDisplay::setShowLatestOnly, this, _1),
00629 parent_category_, this);
00630
00631 showModelName_property_ = property_manager_->createProperty<
00632 rviz::BoolProperty> ("Show Model Name", property_prefix_,
00633 boost::bind(&ModelDisplay::getShowModelName, this), boost::bind(
00634 &ModelDisplay::setShowModelName, this, _1),
00635 parent_category_, this);
00636
00637 showForces_property_ = property_manager_->createProperty<
00638 rviz::BoolProperty> ("Show Forces", property_prefix_,
00639 boost::bind(&ModelDisplay::getShowForces, this), boost::bind(
00640 &ModelDisplay::setShowForces, this, _1),
00641 parent_category_, this);
00642
00643 forceScaling_property_ = property_manager_->createProperty<
00644 rviz::FloatProperty> ("Force scaling", property_prefix_,
00645 boost::bind(&ModelDisplay::getForceScaling, this), boost::bind(
00646 &ModelDisplay::setForceScaling, this, _1),
00647 parent_category_, this);
00648
00649 }
00650
00651 void ModelDisplay::clearDisplay() {
00652
00653
00654 clearMap();
00655 }
00656
00657 bool ModelDisplay::transform(
00658 const articulation_msgs::ModelMsg::ConstPtr& message,
00659 btTransform &transform) {
00660 std::string fixed_frame = vis_manager_->getFixedFrame();
00661
00662 std::string frame_id = message->header.frame_id;
00663 if (frame_id.empty()) {
00664 frame_id = fixed_frame;
00665 }
00666
00667 tf::Stamped<tf::Pose> pose(btTransform(btQuaternion(0, 0, 0, 1), btVector3(
00668 0, 0, 0)), message->header.stamp, frame_id);
00669 try {
00670 vis_manager_->getTFClient()->transformPose(fixed_frame, pose, pose);
00671 } catch (tf::TransformException& e) {
00672 ROS_ERROR( "Error transforming track/model '%d'/'%d' from frame '%s' to frame '%s': %s\n",message->track.id,message->id, frame_id.c_str(), fixed_frame.c_str(), e.what() );
00673 return false;
00674 }
00675
00676 transform = pose;
00677
00678 return (true);
00679 }
00680
00681 bool ModelDisplay::transform(const btTransform &pose, const btVector3 &scaleIn,
00682 Ogre::Vector3& pos, Ogre::Quaternion& orient, Ogre::Vector3& scaleOut) {
00683
00684 pos = Ogre::Vector3(pose.getOrigin().x(), pose.getOrigin().y(),
00685 pose.getOrigin().z());
00686 rviz::robotToOgre(pos);
00687
00688 btQuaternion quat;
00689 pose.getBasis().getRotation(quat);
00690 orient = Ogre::Quaternion::IDENTITY;
00691 rviz::ogreToRobot(orient);
00692 orient = Ogre::Quaternion(quat.w(), quat.x(), quat.y(), quat.z()) * orient;
00693 rviz::robotToOgre(orient);
00694
00695 scaleOut = Ogre::Vector3(scaleIn.x(), scaleIn.y(), scaleIn.z());
00696 rviz::scaleRobotToOgre(scaleOut);
00697
00698 return true;
00699 }
00700
00701 ogre_tools::BillboardLine* ModelDisplay::newBillboardLine() {
00702 if (recycleLines.size() > 0) {
00703 ogre_tools::BillboardLine* line = recycleLines.back();
00704 recycleLines.pop_back();
00705 return (line);
00706 }
00707 return (new ogre_tools::BillboardLine(vis_manager_->getSceneManager(),
00708 scene_node_));
00709 }
00710
00711 ogre_tools::Arrow* ModelDisplay::newArrow() {
00712 if (recycleArrows.size() > 0) {
00713 ogre_tools::Arrow* arrow = recycleArrows.back();
00714 recycleArrows.pop_back();
00715 return (arrow);
00716 }
00717 return (new ogre_tools::Arrow(vis_manager_->getSceneManager(),
00718 scene_node_));
00719 }
00720
00721 ogre_tools::MovableText* ModelDisplay::newText() {
00722 if (recycleText.size() > 0) {
00723 ogre_tools::MovableText* line = recycleText.back();
00724 recycleText.pop_back();
00725 return (line);
00726 }
00727 ogre_tools::MovableText* t = new ogre_tools::MovableText( "test", "Arial", 0.03 );
00728 t->setTextAlignment(ogre_tools::MovableText::H_CENTER, ogre_tools::MovableText::V_BELOW);
00729
00730 Ogre::SceneNode* n = scene_node_->createChildSceneNode();
00731 n->attachObject(t);
00732 return (t);
00733 }
00734
00735
00736
00737 void ModelDisplay::createRectangle(Ogre::Vector3 pos, Ogre::Quaternion orient,
00738 Ogre::Vector3 scale, double w, double h, btVector3 color, std::vector<
00739 ogre_tools::BillboardLine*> &vec) {
00740
00741 ogre_tools::BillboardLine* lines = newBillboardLine();
00742
00743 lines->setPosition(pos);
00744 lines->setOrientation(orient);
00745 lines->setScale(scale);
00746 lines->setColor(color.x(), color.y(), color.z(), alpha_);
00747
00748 lines->clear();
00749 lines->setLineWidth(lineWidth_);
00750 lines->setMaxPointsPerLine(5);
00751 lines->setNumLines(1);
00752
00753 Ogre::Vector3 v;
00754
00755 v = Ogre::Vector3(0, 0, 0);
00756 rviz::robotToOgre(v);
00757 lines->addPoint(v);
00758 v = Ogre::Vector3(w, 0, 0);
00759 rviz::robotToOgre(v);
00760 lines->addPoint(v);
00761 v = Ogre::Vector3(w, h, 0);
00762 rviz::robotToOgre(v);
00763 lines->addPoint(v);
00764 v = Ogre::Vector3(0, h, 0);
00765 rviz::robotToOgre(v);
00766 lines->addPoint(v);
00767 v = Ogre::Vector3(0, 0, 0);
00768 rviz::robotToOgre(v);
00769 lines->addPoint(v);
00770
00771 vec.push_back(lines);
00772 }
00773
00774 void ModelDisplay::createAxes(Ogre::Vector3 pos, Ogre::Quaternion orient,
00775 Ogre::Vector3 scale, btVector3 color, std::vector<
00776 ogre_tools::BillboardLine*> &vec) {
00777
00778 for (int i = 0; i < 3; i++) {
00779 ogre_tools::BillboardLine* lines = newBillboardLine();
00780
00781 lines->clear();
00782 lines->setPosition(pos);
00783 lines->setOrientation(orient);
00784 lines->setScale(scale);
00785
00786 btVector3 hsv = RGB_to_HSV(color);
00787 hsv.setX(i / 3.0);
00788 btVector3 axis_color = HSV_to_RGB(hsv);
00789 lines->setColor(axis_color.x(), axis_color.y(), axis_color.z(), alpha_);
00790
00791 lines->setLineWidth(lineWidth_);
00792 lines->setMaxPointsPerLine(2);
00793 lines->setNumLines(1);
00794
00795 Ogre::Vector3 v;
00796
00797 v = Ogre::Vector3(0, 0, 0);
00798 v[i] = lineWidth_ * 5;
00799 rviz::robotToOgre(v);
00800 lines->addPoint(v);
00801
00802 v = Ogre::Vector3(0, 0, 0);
00803
00804 rviz::robotToOgre(v);
00805 lines->addPoint(v);
00806
00807 vec.push_back(lines);
00808 }
00809 }
00810
00811 void ModelDisplay::createLine(Ogre::Vector3 pos, Ogre::Vector3 old_pos,
00812 Ogre::Vector3 scale, btVector3 color, std::vector<
00813 ogre_tools::BillboardLine*> &vec,bool add_cross,bool with_line) {
00814
00815 ogre_tools::BillboardLine* lines = newBillboardLine();
00816 lines->setPosition(pos);
00817 lines->setOrientation(Ogre::Quaternion(1, 0, 0, 0));
00818 lines->setScale(scale);
00819
00820 lines->setColor(color.x(), color.y(), color.z(), alpha_);
00821 lines->clear();
00822
00823 lines->setLineWidth(lineWidth_);
00824 lines->setMaxPointsPerLine(2);
00825 lines->setNumLines(4);
00826
00827 Ogre::Vector3 v;
00828 if(add_cross) {
00829 for (int i = 0; i < 3; i++) {
00830
00831 v = Ogre::Vector3(0, 0, 0);
00832 v[i] = lineWidth_ * 5;
00833 rviz::robotToOgre(v);
00834 lines->addPoint(v);
00835
00836 v = Ogre::Vector3(0, 0, 0);
00837 v[i] = -lineWidth_ * 5;
00838 rviz::robotToOgre(v);
00839 lines->addPoint(v);
00840 lines->newLine();
00841 }
00842 }
00843
00844 if(with_line) {
00845 v = Ogre::Vector3(0, 0, 0);
00846 rviz::robotToOgre(v);
00847 lines->addPoint(v);
00848
00849 v = old_pos - pos;
00850
00851 lines->addPoint(v);
00852 }
00853
00854 vec.push_back(lines);
00855 }
00856
00857 void ModelDisplay::createArrow(Ogre::Vector3 point2, Ogre::Vector3 point1,
00858 Ogre::Vector3 scale, btVector3 color, std::vector<
00859 ogre_tools::Arrow*> &vec) {
00860
00861 ogre_tools::Arrow* arrow = newArrow();
00862
00863 arrow->setColor(color.x(), color.y(), color.z(), alpha_);
00864
00865 Ogre::Vector3 direction = point2 - point1;
00866 float distance = direction.length();
00867 direction.normalise();
00868 Ogre::Quaternion orient = Ogre::Vector3::NEGATIVE_UNIT_Z.getRotationTo( direction );
00869 arrow->setPosition(point1);
00870 arrow->setOrientation(orient);
00871 arrow->setScale(Ogre::Vector3(1.0f, 1.0f, 1.0f));
00872
00873 float head_length = 0.3*distance;
00874 float shaft_length = distance - head_length;
00875 arrow->set(shaft_length, scale.x, head_length, scale.y*2);
00876
00877 vec.push_back(arrow);
00878 }
00879
00880 void ModelDisplay::createText(Ogre::Vector3 pos, Ogre::Quaternion orient,
00881 Ogre::Vector3 scale, btVector3 color, std::string s, std::vector<
00882 ogre_tools::MovableText*> &vec) {
00883
00884 ogre_tools::MovableText* text = newText();
00885 text->setCharacterHeight(lineWidth_/0.05);
00886 text->setTextAlignment(ogre_tools::MovableText::H_LEFT,ogre_tools::MovableText::V_BELOW);
00887
00888 text->setCaption(s);
00889
00890 text->setColor(Ogre::ColourValue(color.x(), color.y(), color.z(), alpha_));
00891 text->getParentNode()->setPosition(pos);
00892 text->setVisible(true);
00893
00894 vec.push_back(text);
00895 }
00896
00897 void ModelDisplay::setColor(const rviz::Color& color) {
00898 color_ = color;
00899
00900 propertyChanged(color_property_);
00901 }
00902
00903 void ModelDisplay::setAlpha(float a) {
00904 alpha_ = a;
00905
00906 propertyChanged(alpha_property_);
00907 }
00908
00909 void ModelDisplay::setLineWidth(float a) {
00910 lineWidth_ = a;
00911
00912 propertyChanged(lineWidth_property_);
00913 }
00914
00915 void ModelDisplay::setTrackColor(int cs) {
00916 trackColor_ = cs;
00917
00918 propertyChanged(trackColor_property_);
00919 }
00920
00921 void ModelDisplay::setUnique(int cs) {
00922 unique_ = cs;
00923
00924 propertyChanged(unique_property_);
00925 }
00926
00927 void ModelDisplay::setModelColor(int cs) {
00928 modelColor_ = cs;
00929
00930 propertyChanged(modelColor_property_);
00931 }
00932
00933 void ModelDisplay::setPoseColor(int cs) {
00934 poseColor_ = cs;
00935
00936 propertyChanged(poseColor_property_);
00937 }
00938
00939 void ModelDisplay::setModelTypeColor(int cs) {
00940 modelTypeColor_ = cs;
00941
00942 propertyChanged(modelTypeColor_property_);
00943 }
00944
00945 void ModelDisplay::setLikelihoodColor(int cs) {
00946 likelihoodColor_ = cs;
00947
00948 propertyChanged(likelihoodColor_property_);
00949 }
00950
00951 void ModelDisplay::setForceColor(int cs) {
00952 forceColor_ = cs;
00953
00954 propertyChanged(forceColor_property_);
00955 }
00956
00957 btVector3 ModelDisplay::modifyColor(btVector3 color, int colorStyle, float f) {
00958 switch (colorStyle) {
00959 case cs_fixed:
00960 break;
00961 case cs_channel:
00962 break;
00963 case cs_hue:
00964 color = RGB_to_HSV(color);
00965 color.setX(f*0.7);
00966 color = HSV_to_RGB(color);
00967 break;
00968 case cs_brightness:
00969 if (f < 0.5) {
00970 color = color * (f + 0.5);
00971 } else {
00972 color = btVector3(1, 1, 1) - ((btVector3(1, 1, 1) - color) * (0.5
00973 + (1 - f)));
00974 }
00975 break;
00976 }
00977 return (color);
00978 }
00979
00980 void ModelDisplay::setDisplayStyle(int ds) {
00981 displayStyle_ = ds;
00982
00983 propertyChanged(displayStyle_property_);
00984 }
00985
00986 void ModelDisplay::setSampleConfigurations(bool sc) {
00987 sampleConfigurations_ = sc;
00988 propertyChanged(sampleConfigurations_property_);
00989
00990 if(sc) {
00991 projectConfigurations_ = false;
00992 propertyChanged(projectConfigurations_property_);
00993 }
00994 }
00995
00996 void ModelDisplay::setProjectConfigurations(bool sc) {
00997 projectConfigurations_ = sc;
00998 propertyChanged(projectConfigurations_property_);
00999 if(!sc) {
01000 sampleConfigurations_ = false;
01001 propertyChanged(sampleConfigurations_property_);
01002 }
01003 }
01004
01005 void ModelDisplay::setShowJacobians(bool sc) {
01006 showJacobians_ = sc;
01007
01008 propertyChanged(showJacobians_property_);
01009 }
01010
01011 void ModelDisplay::setShowHessians(bool sc) {
01012 showHessians_ = sc;
01013
01014 propertyChanged(showHessians_property_);
01015 }
01016
01017 void ModelDisplay::setShowLatestOnly(bool sc) {
01018 showLatestOnly_ = sc;
01019
01020 propertyChanged(showLatestOnly_property_);
01021 }
01022
01023 void ModelDisplay::setSampleDensity(float sc) {
01024 sampleDensity_ = sc;
01025
01026 propertyChanged(sampleDensity_property_);
01027 }
01028
01029 void ModelDisplay::setShowModelName(bool sc) {
01030 showModelName_ = sc;
01031
01032 propertyChanged(showModelName_property_);
01033 }
01034
01035 void ModelDisplay::setShowForces(bool sc) {
01036 showForces_ = sc;
01037
01038 propertyChanged(showForces_property_);
01039 }
01040
01041 void ModelDisplay::setForceScaling(float sc) {
01042 forceScaling_ = sc;
01043
01044 propertyChanged(forceScaling_property_);
01045 }
01046
01047
01048 }
01049
