00001 #include "articulation_display.h"
00002 #include "rviz/common.h"
00003 #include "rviz/visualization_manager.h"
00004 #include "rviz/properties/property.h"
00005 #include "rviz/properties/property_manager.h"
00006
00007 #include <ogre_tools/axes.h>
00008
00009 #include <tf/transform_listener.h>
00010 #include <OGRE/OgreSceneNode.h>
00011 #include <OGRE/OgreSceneManager.h>
00012
00013 #include "utils.h"
00014
00015 namespace articulation_rviz_plugin {
00016
00017 TrackDisplay::TrackDisplay(const std::string& name,
00018 rviz::VisualizationManager* manager) :
00019 Display(name, manager), track_topic_("/track"), color_(0.5f, 0.0f, 1.0f),
00020 alpha_(0.5f), lineWidth_(0.003f)
00021 ,trackColor_(cs_fixed),poseColor_(cs_fixed),
00022 displayStyle_(ds_line),
00023 tf_filter_(
00024 *manager->getTFClient(), "", 1000, update_nh_) {
00025 scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00026
00027 tf_filter_.connectInput(sub_);
00028 tf_filter_.registerCallback(boost::bind(&TrackDisplay::incomingTrack, this,
00029 _1));
00030 }
00031
00032 TrackDisplay::~TrackDisplay() {
00033 unsubscribe();
00034
00035 }
00036
00037 void TrackDisplay::clearMap() {
00038 std::map<int, std::vector<ogre_tools::BillboardLine*> >::iterator it;
00039 for (it = lines.begin(); it != lines.end(); it++) {
00040 clearVector(it->second);
00041 }
00042 }
00043
00044 void TrackDisplay::clearVector(std::vector<ogre_tools::BillboardLine*>& vec) {
00045 for (size_t j = 0; j < vec.size(); j++) {
00046 delete vec[j];
00047 }
00048 vec.clear();
00049 }
00050
00051 void TrackDisplay::setTopic(const std::string& topic) {
00052 unsubscribe();
00053 track_topic_ = topic;
00054 subscribe();
00055
00056 propertyChanged(topic_property_);
00057 }
00058
00059 void TrackDisplay::onEnable() {
00060 subscribe();
00061
00062 scene_node_->setVisible(true);
00063 }
00064
00065 void TrackDisplay::onDisable() {
00066 unsubscribe();
00067 scene_node_->setVisible(false);
00068 clearDisplay();
00069 }
00070
00071 void TrackDisplay::reset() {
00072 clearDisplay();
00073 }
00074
00075 void TrackDisplay::subscribe() {
00076 if (!isEnabled()) {
00077 return;
00078 }
00079
00080 if (!track_topic_.empty()) {
00081 sub_.subscribe(update_nh_, track_topic_, 0);
00082 }
00083
00084 }
00085
00086 void TrackDisplay::unsubscribe() {
00087 sub_.unsubscribe();
00088 }
00089
00090 void TrackDisplay::incomingTrack(
00091 const articulation_msgs::TrackMsg::ConstPtr& msg) {
00092 incoming_track_message_ = msg;
00093 boost::mutex::scoped_lock lock(queue_mutex_);
00094
00095 message_queue_.push_back(msg);
00096 }
00097
00098 void TrackDisplay::update(float wall_dt, float ros_dt) {
00099 V_TrackMsg local_queue;
00100
00101 {
00102 boost::mutex::scoped_lock lock(queue_mutex_);
00103
00104 local_queue.swap(message_queue_);
00105 }
00106
00107 for (size_t t = 0; t < local_queue.size(); t++) {
00108 const articulation_msgs::TrackMsg::ConstPtr& track_message =
00109 local_queue[t];
00110
00111 for(size_t l=0;l<lines[track_message->id].size();l++)
00112 lines[track_message->id][l]->clear();
00113 recycleLines.insert(recycleLines.begin(),
00114 lines[track_message->id].begin(),
00115 lines[track_message->id].end());
00116
00117 lines[track_message->id].clear();
00118 }
00119
00120 for (size_t t = 0; t < local_queue.size(); t++) {
00121 const articulation_msgs::TrackMsg::ConstPtr& track_message =
00122 local_queue[t];
00123
00124 btTransform framePose;
00125 btVector3 lineScale(lineWidth_, lineWidth_, lineWidth_);
00126 transform(track_message, framePose);
00127
00128 int channel_w = -1;
00129 int channel_h = -1;
00130 for (size_t i = 0; i < track_message->channels.size(); i++) {
00131 if (track_message->channels[i].name == "width")
00132 channel_w = (int) i;
00133 if (track_message->channels[i].name == "height")
00134 channel_h = (int) i;
00135 }
00136
00137 Ogre::Vector3 old_pos(0,0,0);
00138 for (size_t i = 0; i < track_message->pose.size(); i++) {
00139 const geometry_msgs::Pose& geo_pose = track_message->pose[i];
00140
00141 btTransform rectangle_pose(btQuaternion(geo_pose.orientation.x,
00142 geo_pose.orientation.y, geo_pose.orientation.z,
00143 geo_pose.orientation.w), btVector3(geo_pose.position.x,
00144 geo_pose.position.y, geo_pose.position.z));
00145
00146 Ogre::Vector3 pos, scale;
00147 Ogre::Quaternion orient;
00148 transform(framePose * rectangle_pose, lineScale, pos, orient, scale);
00149
00150 btVector3 color(color_.r_, color_.g_, color_.b_);
00151
00152 double f =track_message->id / 7.0;
00153 color = modifyColor( color, trackColor_, f - floor(f) );
00154 color = modifyColor( color, poseColor_, i / (double)track_message->pose.size() );
00155
00156 if(displayStyle_ == ds_line) {
00157 if(i==0) old_pos = pos;
00158 createLine(pos, old_pos, scale,
00159 color,
00160 lines[track_message->id],false);
00161 old_pos = pos;
00162 } else
00163 if(displayStyle_ == ds_cross_line) {
00164 if(i==0) old_pos = pos;
00165 createLine(pos, old_pos, scale,
00166 color,
00167 lines[track_message->id],true);
00168 old_pos = pos;
00169 } else
00170 if(displayStyle_ == ds_axes) {
00171 createAxes(pos, orient, scale,
00172 color,
00173 lines[track_message->id]);
00174 } else
00175 if(displayStyle_ == ds_rectangle) {
00176 createRectangle(pos, orient, scale,
00177 channel_w==-1? lineWidth_*5 : track_message->channels[channel_w].values[i],
00178 channel_h==-1? lineWidth_*5 : track_message->channels[channel_h].values[i],
00179 color,
00180 lines[track_message->id]);
00181 }
00182 }
00183 }
00184 }
00185
00186 void TrackDisplay::targetFrameChanged() {
00187 }
00188
00189 void TrackDisplay::fixedFrameChanged() {
00190 tf_filter_.setTargetFrame(fixed_frame_);
00191
00192 clearDisplay();
00193 }
00194
00195 void TrackDisplay::createProperties() {
00196 topic_property_ = property_manager_->createProperty<
00197 rviz::ROSTopicStringProperty> ("Topic", property_prefix_,
00198 boost::bind(&TrackDisplay::getTopic, this), boost::bind(
00199 &TrackDisplay::setTopic, this, _1), parent_category_, this);
00200 rviz::ROSTopicStringPropertyPtr topic_prop = topic_property_.lock();
00201 topic_prop->setMessageType(articulation_msgs::TrackMsg::__s_getDataType());
00202
00203 color_property_ = property_manager_->createProperty<rviz::ColorProperty> (
00204 "Color", property_prefix_, boost::bind(&TrackDisplay::getColor,
00205 this), boost::bind(&TrackDisplay::setColor, this, _1),
00206 parent_category_, this);
00207
00208 alpha_property_ = property_manager_->createProperty<rviz::FloatProperty> (
00209 "Alpha", property_prefix_, boost::bind(&TrackDisplay::getAlpha,
00210 this), boost::bind(&TrackDisplay::setAlpha, this, _1),
00211 parent_category_, this);
00212
00213 lineWidth_property_ = property_manager_->createProperty<rviz::FloatProperty> (
00214 "Line Width", property_prefix_, boost::bind(&TrackDisplay::getLineWidth,
00215 this), boost::bind(&TrackDisplay::setLineWidth, this, _1),
00216 parent_category_, this);
00217
00218 trackColor_property_ = property_manager_->createProperty<rviz::EnumProperty>( "Track Color", property_prefix_, boost::bind( &TrackDisplay::getTrackColor, this ),
00219 boost::bind( &TrackDisplay::setTrackColor, this, _1 ), parent_category_, this );
00220 rviz::EnumPropertyPtr enum_prop1 = trackColor_property_.lock();
00221 enum_prop1->addOption( "fixed", cs_fixed );
00222
00223 enum_prop1->addOption( "hue", cs_hue );
00224 enum_prop1->addOption( "brightness", cs_brightness );
00225
00226 poseColor_property_ = property_manager_->createProperty<rviz::EnumProperty>( "Pose Color", property_prefix_, boost::bind( &TrackDisplay::getPoseColor, this ),
00227 boost::bind( &TrackDisplay::setPoseColor, this, _1 ), parent_category_, this );
00228 rviz::EnumPropertyPtr enum_prop2 = poseColor_property_.lock();
00229 enum_prop2->addOption( "fixed", cs_fixed );
00230
00231 enum_prop2->addOption( "hue", cs_hue );
00232 enum_prop2->addOption( "brightness", cs_brightness );
00233
00234 displayStyle_property_ = property_manager_->createProperty<rviz::EnumProperty>( "Style", property_prefix_, boost::bind( &TrackDisplay::getDisplayStyle, this ),
00235 boost::bind( &TrackDisplay::setDisplayStyle, this, _1 ), parent_category_, this );
00236 rviz::EnumPropertyPtr enum_prop3 = displayStyle_property_.lock();
00237 enum_prop3->addOption( "line", ds_line );
00238 enum_prop3->addOption( "cross line", ds_cross_line );
00239 enum_prop3->addOption( "axes", ds_axes );
00240 enum_prop3->addOption( "rectangle", ds_rectangle );
00241
00242 }
00243
00244 void TrackDisplay::clearDisplay() {
00245
00246
00247 clearMap();
00248 }
00249
00250 bool TrackDisplay::transform(
00251 const articulation_msgs::TrackMsg::ConstPtr& message,
00252 btTransform &transform) {
00253 std::string fixed_frame = vis_manager_->getFixedFrame();
00254
00255 std::string frame_id = message->header.frame_id;
00256 if (frame_id.empty()) {
00257 frame_id = fixed_frame;
00258 }
00259
00260 tf::Stamped<tf::Pose> pose(btTransform(btQuaternion(0, 0, 0, 1), btVector3(
00261 0, 0, 0)), message->header.stamp, frame_id);
00262 try {
00263 vis_manager_->getTFClient()->transformPose(fixed_frame, pose, pose);
00264 } catch (tf::TransformException& e) {
00265 ROS_ERROR( "Error transforming track '%d' from frame '%s' to frame '%s': %s\n",message->id, frame_id.c_str(), fixed_frame.c_str(), e.what() );
00266 return false;
00267 }
00268
00269 transform = pose;
00270
00271 return (true);
00272 }
00273
00274 bool TrackDisplay::transform(const btTransform &pose, const btVector3 &scaleIn,
00275 Ogre::Vector3& pos, Ogre::Quaternion& orient, Ogre::Vector3& scaleOut) {
00276
00277 pos = Ogre::Vector3(pose.getOrigin().x(), pose.getOrigin().y(),
00278 pose.getOrigin().z());
00279 rviz::robotToOgre(pos);
00280
00281 btQuaternion quat;
00282 pose.getBasis().getRotation(quat);
00283 orient = Ogre::Quaternion::IDENTITY;
00284 rviz::ogreToRobot(orient);
00285 orient = Ogre::Quaternion(quat.w(), quat.x(), quat.y(), quat.z()) * orient;
00286 rviz::robotToOgre(orient);
00287
00288 scaleOut = Ogre::Vector3(scaleIn.x(), scaleIn.y(), scaleIn.z());
00289 rviz::scaleRobotToOgre(scaleOut);
00290
00291 return true;
00292 }
00293
00294 ogre_tools::BillboardLine* TrackDisplay::newBillboardLine() {
00295 if (recycleLines.size() > 0) {
00296 ogre_tools::BillboardLine* line = recycleLines.back();
00297 recycleLines.pop_back();
00298 return (line);
00299 }
00300 return (new ogre_tools::BillboardLine(vis_manager_->getSceneManager(),
00301 scene_node_));
00302 }
00303
00304 void TrackDisplay::createRectangle(Ogre::Vector3 pos, Ogre::Quaternion orient,
00305 Ogre::Vector3 scale, double w, double h, btVector3 color, std::vector<
00306 ogre_tools::BillboardLine*> &vec) {
00307
00308 ogre_tools::BillboardLine* lines = newBillboardLine();
00309
00310 lines->setPosition(pos);
00311 lines->setOrientation(orient);
00312 lines->setScale(scale);
00313 lines->setColor(color.x(), color.y(), color.z(), alpha_);
00314
00315 lines->clear();
00316 lines->setLineWidth(lineWidth_);
00317 lines->setMaxPointsPerLine(5);
00318 lines->setNumLines(1);
00319
00320 Ogre::Vector3 v;
00321
00322 v = Ogre::Vector3(0, 0, 0);
00323 rviz::robotToOgre(v);
00324 lines->addPoint(v);
00325 v = Ogre::Vector3(w, 0, 0);
00326 rviz::robotToOgre(v);
00327 lines->addPoint(v);
00328 v = Ogre::Vector3(w, h, 0);
00329 rviz::robotToOgre(v);
00330 lines->addPoint(v);
00331 v = Ogre::Vector3(0, h, 0);
00332 rviz::robotToOgre(v);
00333 lines->addPoint(v);
00334 v = Ogre::Vector3(0, 0, 0);
00335 rviz::robotToOgre(v);
00336 lines->addPoint(v);
00337
00338 vec.push_back(lines);
00339 }
00340
00341 void TrackDisplay::createAxes(Ogre::Vector3 pos, Ogre::Quaternion orient,
00342 Ogre::Vector3 scale, btVector3 color, std::vector<
00343 ogre_tools::BillboardLine*> &vec) {
00344
00345
00346 for(int i=0;i<3;i++) {
00347 ogre_tools::BillboardLine* lines = newBillboardLine();
00348
00349 lines->clear();
00350 lines->setPosition(pos);
00351 lines->setOrientation(orient);
00352 lines->setScale(scale);
00353
00354 btVector3 hsv = RGB_to_HSV(color);
00355 hsv.setX(i/3.0);
00356 btVector3 axis_color = HSV_to_RGB(hsv);
00357 lines->setColor(axis_color.x(), axis_color.y(), axis_color.z(), alpha_);
00358
00359 lines->setLineWidth(lineWidth_);
00360 lines->setMaxPointsPerLine(2);
00361 lines->setNumLines(1);
00362
00363 Ogre::Vector3 v;
00364
00365 v = Ogre::Vector3(0, 0, 0);
00366 v[i] = lineWidth_ * 5;
00367 rviz::robotToOgre(v);
00368 lines->addPoint(v);
00369
00370 v = Ogre::Vector3(0, 0, 0);
00371 v[i] = -lineWidth_ * 5;
00372 rviz::robotToOgre(v);
00373 lines->addPoint(v);
00374
00375 vec.push_back(lines);
00376 }
00377 }
00378
00379 void TrackDisplay::createLine(Ogre::Vector3 pos, Ogre::Vector3 old_pos,
00380 Ogre::Vector3 scale, btVector3 color, std::vector<
00381 ogre_tools::BillboardLine*> &vec,bool add_cross) {
00382
00383 ogre_tools::BillboardLine* lines = newBillboardLine();
00384 lines->setPosition(pos);
00385 lines->setOrientation(Ogre::Quaternion(1,0,0,0));
00386 lines->setScale(scale);
00387
00388 lines->setColor(color.x(), color.y(), color.z(), alpha_);
00389 lines->clear();
00390
00391 lines->setLineWidth(lineWidth_);
00392 lines->setMaxPointsPerLine(2);
00393 lines->setNumLines(4);
00394
00395 Ogre::Vector3 v;
00396 if(add_cross) {
00397 for(int i=0;i<3;i++) {
00398
00399 v = Ogre::Vector3(0, 0, 0);
00400 v[i] = lineWidth_ * 5;
00401 rviz::robotToOgre(v);
00402 lines->addPoint(v);
00403
00404 v = Ogre::Vector3(0, 0, 0);
00405 v[i] = -lineWidth_ * 5;
00406 rviz::robotToOgre(v);
00407 lines->addPoint(v);
00408 lines->newLine();
00409 }
00410 }
00411
00412 v = Ogre::Vector3(0, 0, 0);
00413 rviz::robotToOgre(v);
00414 lines->addPoint(v);
00415
00416 v = old_pos - pos;
00417
00418 lines->addPoint(v);
00419
00420 vec.push_back(lines);
00421 }
00422
00423
00424 void TrackDisplay::setColor(const rviz::Color& color) {
00425 color_ = color;
00426
00427 propertyChanged(color_property_);
00428 }
00429
00430 void TrackDisplay::setAlpha(float a) {
00431 alpha_ = a;
00432
00433 propertyChanged(alpha_property_);
00434 }
00435
00436 void TrackDisplay::setLineWidth(float a) {
00437 lineWidth_ = a;
00438
00439 propertyChanged(lineWidth_property_);
00440 }
00441
00442 void TrackDisplay::setTrackColor(int cs) {
00443 trackColor_ = cs;
00444
00445 propertyChanged(trackColor_property_);
00446 }
00447
00448 void TrackDisplay::setPoseColor(int cs) {
00449 poseColor_ = cs;
00450
00451 propertyChanged(poseColor_property_);
00452 }
00453
00454 btVector3 TrackDisplay::modifyColor( btVector3 color, int colorStyle, float f ) {
00455 switch(colorStyle) {
00456 case cs_fixed:
00457 break;
00458 case cs_channel:
00459 break;
00460 case cs_hue:
00461 color = RGB_to_HSV(color);
00462 color.setX( f );
00463 color = HSV_to_RGB(color);
00464 break;
00465 case cs_brightness:
00466 if(f < 0.5) {
00467 color = color * (f+0.5);
00468 } else {
00469 color = btVector3(1,1,1) - ( (btVector3(1,1,1) - color) * (0.5 + (1-f)) );
00470 }
00471 break;
00472 }
00473 return(color);
00474 }
00475
00476 void TrackDisplay::setDisplayStyle(int ds) {
00477 displayStyle_ = ds;
00478
00479 propertyChanged(displayStyle_property_);
00480 }
00481
00482
00483
00484 }
00485
