$search
00001 /* 00002 * Copyright (c) 2008, Willow Garage, Inc. 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 00030 #include "cob_3d_mapping_rviz_plugins/shape_display.h" 00031 #include "rviz/visualization_manager.h" 00032 #include "rviz/properties/property_manager.h" 00033 #include "rviz/properties/property.h" 00034 #include "rviz/selection/selection_manager.h" 00035 #include "rviz/frame_manager.h" 00036 #include "rviz/validate_floats.h" 00037 00038 /*#include "markers/shape_marker.h" 00039 #include "markers/arrow_marker.h" 00040 #include "markers/line_list_marker.h" 00041 #include "markers/line_strip_marker.h" 00042 #include "markers/points_marker.h" 00043 #include "markers/text_view_facing_marker.h" 00044 #include "markers/mesh_resource_marker.h" 00045 #include "markers/triangle_list_marker.h"*/ 00046 #include "cob_3d_mapping_rviz_plugins/shape_marker.h" 00047 00048 #include <ogre_tools/arrow.h> 00049 #include <ogre_tools/shape.h> 00050 #include <ogre_tools/billboard_line.h> 00051 00052 #include <tf/transform_listener.h> 00053 00054 #include <OGRE/OgreSceneNode.h> 00055 #include <OGRE/OgreSceneManager.h> 00056 00057 namespace rviz 00058 { 00059 00063 00064 ShapeDisplay::ShapeDisplay( const std::string& name, VisualizationManager* manager ) 00065 : Display( name, manager ) 00066 , tf_filter_(*manager->getTFClient(), "", 100, update_nh_) 00067 , marker_topic_("visualization_marker") 00068 { 00069 scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode(); 00070 00071 tf_filter_.connectInput(sub_); 00072 tf_filter_.registerCallback(boost::bind(&ShapeDisplay::incomingMarker, this, _1)); 00073 tf_filter_.registerFailureCallback(boost::bind(&ShapeDisplay::failedMarker, this, _1, _2)); 00074 } 00075 00076 ShapeDisplay::~ShapeDisplay() 00077 { 00078 unsubscribe(); 00079 00080 clearMarkers(); 00081 } 00082 00083 /*MarkerBasePtr ShapeDisplay::getMarker(MarkerID id) 00084 { 00085 M_IDToMarker::iterator it = markers_.find(id); 00086 if (it != markers_.end()) 00087 { 00088 return it->second; 00089 } 00090 00091 return MarkerBasePtr(); 00092 }*/ 00093 00094 void ShapeDisplay::clearMarkers() 00095 { 00096 markers_.clear(); 00097 //markers_with_expiration_.clear(); 00098 //frame_locked_markers_.clear(); 00099 tf_filter_.clear(); 00100 00101 if (property_manager_) 00102 { 00103 M_Namespace::iterator it = namespaces_.begin(); 00104 M_Namespace::iterator end = namespaces_.end(); 00105 for (; it != end; ++it) 00106 { 00107 property_manager_->deleteProperty(it->second.prop.lock()); 00108 } 00109 } 00110 00111 namespaces_.clear(); 00112 } 00113 00114 void ShapeDisplay::onEnable() 00115 { 00116 subscribe(); 00117 00118 scene_node_->setVisible( true ); 00119 } 00120 00121 void ShapeDisplay::onDisable() 00122 { 00123 unsubscribe(); 00124 tf_filter_.clear(); 00125 00126 clearMarkers(); 00127 00128 scene_node_->setVisible( false ); 00129 } 00130 00131 void ShapeDisplay::setMarkerTopic(const std::string& topic) 00132 { 00133 unsubscribe(); 00134 marker_topic_ = topic; 00135 subscribe(); 00136 00137 propertyChanged(marker_topic_property_); 00138 } 00139 00140 void ShapeDisplay::subscribe() 00141 { 00142 if ( !isEnabled() ) 00143 { 00144 return; 00145 } 00146 00147 if (!marker_topic_.empty()) 00148 { 00149 array_sub_.shutdown(); 00150 sub_.unsubscribe(); 00151 00152 try 00153 { 00154 sub_.subscribe(update_nh_, marker_topic_, 1000); 00155 array_sub_ = update_nh_.subscribe(marker_topic_ + "_array", 1000, &ShapeDisplay::incomingMarkerArray, this); 00156 setStatus(status_levels::Ok, "Topic", "OK"); 00157 } 00158 catch (ros::Exception& e) 00159 { 00160 setStatus(status_levels::Error, "Topic", std::string("Error subscribing: ") + e.what()); 00161 } 00162 } 00163 } 00164 00165 void ShapeDisplay::unsubscribe() 00166 { 00167 sub_.unsubscribe(); 00168 array_sub_.shutdown(); 00169 } 00170 00171 void ShapeDisplay::deleteMarker(MarkerID id) 00172 { 00173 /*deleteMarkerStatus(id); 00174 00175 M_IDToMarker::iterator it = markers_.find(id); 00176 if (it != markers_.end()) 00177 { 00178 markers_with_expiration_.erase(it->second); 00179 frame_locked_markers_.erase(it->second); 00180 markers_.erase(it); 00181 }*/ 00182 } 00183 00184 void ShapeDisplay::setNamespaceEnabled(const std::string& ns, bool enabled) 00185 { 00186 /*M_Namespace::iterator it = namespaces_.find(ns); 00187 if (it != namespaces_.end()) 00188 { 00189 it->second.enabled = enabled; 00190 00191 std::vector<MarkerID> to_delete; 00192 00193 // TODO: this is inefficient, should store every in-use id per namespace and lookup by that 00194 M_IDToMarker::iterator marker_it = markers_.begin(); 00195 M_IDToMarker::iterator marker_end = markers_.end(); 00196 for (; marker_it != marker_end; ++marker_it) 00197 { 00198 if (marker_it->first.first == ns) 00199 { 00200 to_delete.push_back(marker_it->first); 00201 } 00202 } 00203 00204 { 00205 std::vector<MarkerID>::iterator it = to_delete.begin(); 00206 std::vector<MarkerID>::iterator end = to_delete.end(); 00207 for (; it != end; ++it) 00208 { 00209 deleteMarker(*it); 00210 } 00211 } 00212 }*/ 00213 } 00214 00215 bool ShapeDisplay::isNamespaceEnabled(const std::string& ns) 00216 { 00217 M_Namespace::iterator it = namespaces_.find(ns); 00218 if (it != namespaces_.end()) 00219 { 00220 return it->second.enabled; 00221 } 00222 00223 return true; 00224 } 00225 00226 void ShapeDisplay::setMarkerStatus(MarkerID id, StatusLevel level, const std::string& text) 00227 { 00228 std::stringstream ss; 00229 ss << id.first << "/" << id.second; 00230 std::string marker_name = ss.str(); 00231 setStatus(level, marker_name, text); 00232 } 00233 00234 void ShapeDisplay::deleteMarkerStatus(MarkerID id) 00235 { 00236 std::stringstream ss; 00237 ss << id.first << "/" << id.second; 00238 std::string marker_name = ss.str(); 00239 deleteStatus(marker_name); 00240 } 00241 00242 void ShapeDisplay::incomingMarkerArray(const cob_3d_mapping_msgs::ShapeArray::ConstPtr& array) 00243 { 00244 markers_.clear(); 00245 std::vector<cob_3d_mapping_msgs::Shape>::const_iterator it = array->shapes.begin(); 00246 std::vector<cob_3d_mapping_msgs::Shape>::const_iterator end = array->shapes.end(); 00247 for (; it != end; ++it) 00248 { 00249 const cob_3d_mapping_msgs::Shape& marker = *it; 00250 tf_filter_.add(cob_3d_mapping_msgs::Shape::Ptr(new cob_3d_mapping_msgs::Shape(marker))); 00251 } 00252 } 00253 00254 void ShapeDisplay::incomingMarker( const cob_3d_mapping_msgs::Shape::ConstPtr& marker ) 00255 { 00256 boost::mutex::scoped_lock lock(queue_mutex_); 00257 00258 markers_.clear(); 00259 message_queue_.push_back(marker); 00260 } 00261 00262 void ShapeDisplay::failedMarker(const cob_3d_mapping_msgs::Shape::ConstPtr& marker, tf::FilterFailureReason reason) 00263 { 00264 //std::string error = FrameManager::instance()->discoverFailureReason(marker->header.frame_id, marker->header.stamp, marker->__connection_header ? (*marker->__connection_header)["callerid"] : "unknown", reason); 00265 //setMarkerStatus(MarkerID(marker->ns, marker->id), status_levels::Error, error); 00266 } 00267 00268 bool validateFloats(const cob_3d_mapping_msgs::Shape& msg) 00269 { 00270 /*bool valid = true; 00271 valid = valid && validateFloats(msg.pose); 00272 valid = valid && validateFloats(msg.scale); 00273 valid = valid && validateFloats(msg.color); 00274 valid = valid && validateFloats(msg.points); 00275 return valid;*/ 00276 return false; 00277 } 00278 00279 void ShapeDisplay::processMessage( const cob_3d_mapping_msgs::Shape::ConstPtr& message ) 00280 { 00281 /*if (!validateFloats(*message)) 00282 { 00283 setMarkerStatus(MarkerID(message->ns, message->id), status_levels::Error, "Contains invalid floating point values (nans or infs)"); 00284 return; 00285 } 00286 00287 switch ( message->action ) 00288 { 00289 case cob_3d_mapping_msgs::Shape::ADD:*/ 00290 processAdd( message ); 00291 /*break; 00292 00293 case cob_3d_mapping_msgs::Shape::DELETE: 00294 processDelete( message ); 00295 break; 00296 00297 default: 00298 ROS_ERROR( "Unknown marker action: %d\n", message->action ); 00299 }*/ 00300 } 00301 00302 void ShapeDisplay::processAdd( const cob_3d_mapping_msgs::Shape::ConstPtr& message ) 00303 { 00304 // 00305 /*M_Namespace::iterator ns_it = namespaces_.find(message->ns); 00306 if (ns_it == namespaces_.end()) 00307 { 00308 Namespace ns; 00309 ns.name = message->ns; 00310 ns.enabled = true; 00311 00312 if (property_manager_) 00313 { 00314 ns.prop = property_manager_->createProperty<BoolProperty>(ns.name, property_prefix_, boost::bind(&ShapeDisplay::isNamespaceEnabled, this, ns.name), 00315 boost::bind(&ShapeDisplay::setNamespaceEnabled, this, ns.name, _1), namespaces_category_, this); 00316 setPropertyHelpText(ns.prop, "Enable/disable all markers in this namespace."); 00317 } 00318 00319 ns_it = namespaces_.insert(std::make_pair(ns.name, ns)).first; 00320 } 00321 00322 if (!ns_it->second.enabled) 00323 { 00324 return; 00325 } 00326 00327 deleteMarkerStatus(MarkerID(message->ns, message->id));*/ 00328 00329 bool create = true; 00330 ShapeBasePtr marker; 00331 00332 /*M_IDToMarker::iterator it = markers_.find( MarkerID(message->ns, message->id) ); 00333 if ( it != markers_.end() ) 00334 { 00335 marker = it->second; 00336 markers_with_expiration_.erase(marker); 00337 if ( message->type == marker->getMessage()->type ) 00338 { 00339 create = false; 00340 } 00341 else 00342 { 00343 markers_.erase( it ); 00344 } 00345 } 00346 00347 if ( create ) 00348 { 00349 switch ( message->type ) 00350 { 00351 case cob_3d_mapping_msgs::Shape::CUBE: 00352 case cob_3d_mapping_msgs::Shape::CYLINDER: 00353 case cob_3d_mapping_msgs::Shape::SPHERE: 00354 { 00355 //marker.reset(new ShapeMarker(this, vis_manager_, scene_node_)); 00356 } 00357 break; 00358 00359 case cob_3d_mapping_msgs::Shape::ARROW: 00360 { 00361 //marker.reset(new ArrowMarker(this, vis_manager_, scene_node_)); 00362 } 00363 break; 00364 00365 case cob_3d_mapping_msgs::Shape::LINE_STRIP: 00366 { 00367 //marker.reset(new LineStripMarker(this, vis_manager_, scene_node_)); 00368 } 00369 break; 00370 case cob_3d_mapping_msgs::Shape::LINE_LIST: 00371 { 00372 //marker.reset(new LineListMarker(this, vis_manager_, scene_node_)); 00373 } 00374 break; 00375 case cob_3d_mapping_msgs::Shape::SPHERE_LIST: 00376 case cob_3d_mapping_msgs::Shape::CUBE_LIST: 00377 case cob_3d_mapping_msgs::Shape::POINTS: 00378 { 00379 //marker.reset(new PointsMarker(this, vis_manager_, scene_node_)); 00380 } 00381 break; 00382 case cob_3d_mapping_msgs::Shape::TEXT_VIEW_FACING: 00383 { 00384 //marker.reset(new TextViewFacingMarker(this, vis_manager_, scene_node_)); 00385 } 00386 break; 00387 case cob_3d_mapping_msgs::Shape::MESH_RESOURCE: 00388 { 00389 //marker.reset(new MeshResourceMarker(this, vis_manager_, scene_node_)); 00390 } 00391 break; 00392 00393 case cob_3d_mapping_msgs::Shape::TRIANGLE_LIST: 00394 { 00395 //marker.reset(new TriangleListMarker(this, vis_manager_, scene_node_)); 00396 } 00397 break; 00398 default: 00399 ROS_ERROR( "Unknown marker type: %d", message->type ); 00400 }*/ 00401 00402 marker.reset(new ShapeMarker(this, vis_manager_, scene_node_)); 00403 markers_.push_back(marker); 00404 //} 00405 00406 if (marker) 00407 { 00408 marker->setMessage(message); 00409 00410 /*if (message->lifetime.toSec() > 0.0001f) 00411 { 00412 markers_with_expiration_.insert(marker); 00413 } 00414 00415 if (message->frame_locked) 00416 { 00417 frame_locked_markers_.insert(marker); 00418 }*/ 00419 00420 causeRender(); 00421 } 00422 } 00423 00424 void ShapeDisplay::processDelete( const cob_3d_mapping_msgs::Shape::ConstPtr& message ) 00425 { 00426 //deleteMarker(MarkerID(message->ns, message->id)); 00427 //causeRender(); 00428 } 00429 00430 void ShapeDisplay::update(float wall_dt, float ros_dt) 00431 { 00432 V_MarkerMessage local_queue; 00433 00434 { 00435 boost::mutex::scoped_lock lock(queue_mutex_); 00436 00437 local_queue.swap( message_queue_ ); 00438 } 00439 00440 if ( !local_queue.empty() ) 00441 { 00442 V_MarkerMessage::iterator message_it = local_queue.begin(); 00443 V_MarkerMessage::iterator message_end = local_queue.end(); 00444 for ( ; message_it != message_end; ++message_it ) 00445 { 00446 cob_3d_mapping_msgs::Shape::ConstPtr& marker = *message_it; 00447 00448 processMessage( marker ); 00449 } 00450 } 00451 00452 /*{ 00453 S_MarkerBase::iterator it = markers_with_expiration_.begin(); 00454 S_MarkerBase::iterator end = markers_with_expiration_.end(); 00455 for (; it != end;) 00456 { 00457 MarkerBasePtr marker = *it; 00458 if (marker->expired()) 00459 { 00460 S_MarkerBase::iterator copy = it; 00461 ++it; 00462 deleteMarker(marker->getID()); 00463 } 00464 else 00465 { 00466 ++it; 00467 } 00468 } 00469 } 00470 00471 { 00472 S_MarkerBase::iterator it = frame_locked_markers_.begin(); 00473 S_MarkerBase::iterator end = frame_locked_markers_.end(); 00474 for (; it != end; ++it) 00475 { 00476 MarkerBasePtr marker = *it; 00477 marker->updateFrameLocked(); 00478 } 00479 }*/ 00480 } 00481 00482 void ShapeDisplay::targetFrameChanged() 00483 { 00484 } 00485 00486 void ShapeDisplay::fixedFrameChanged() 00487 { 00488 tf_filter_.setTargetFrame( fixed_frame_ ); 00489 00490 clearMarkers(); 00491 } 00492 00493 void ShapeDisplay::reset() 00494 { 00495 Display::reset(); 00496 clearMarkers(); 00497 } 00498 00499 void ShapeDisplay::createProperties() 00500 { 00501 marker_topic_property_ = property_manager_->createProperty<ROSTopicStringProperty>( "Marker Topic", property_prefix_, boost::bind( &ShapeDisplay::getMarkerTopic, this ), 00502 boost::bind( &ShapeDisplay::setMarkerTopic, this, _1 ), parent_category_, this ); 00503 setPropertyHelpText(marker_topic_property_, "cob_3d_mapping_msgs::Shape topic to subscribe to. <topic>_array will also automatically be subscribed with type cob_3d_mapping_msgs::ShapeArray."); 00504 ROSTopicStringPropertyPtr topic_prop = marker_topic_property_.lock(); 00505 topic_prop->setMessageType(ros::message_traits::datatype<cob_3d_mapping_msgs::Shape>()); 00506 00507 namespaces_category_ = property_manager_->createCategory("Namespaces", property_prefix_, parent_category_, this); 00508 } 00509 00510 } // namespace rviz