$search
00001 /* 00002 * Copyright (c) 2011, 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 * Author: David Gossow 00030 */ 00031 00032 #ifndef INTERACTIVE_MARKER_SERVER 00033 #define INTERACTIVE_MARKER_SERVER 00034 00035 #include <visualization_msgs/InteractiveMarkerUpdate.h> 00036 #include <visualization_msgs/InteractiveMarkerFeedback.h> 00037 00038 #include <boost/scoped_ptr.hpp> 00039 #include <boost/thread/thread.hpp> 00040 #include <boost/thread/recursive_mutex.hpp> 00041 00042 #include <ros/ros.h> 00043 #include <ros/callback_queue.h> 00044 00045 00046 #include <boost/function.hpp> 00047 #include <boost/unordered_map.hpp> 00048 00049 namespace interactive_markers 00050 { 00051 00056 class InteractiveMarkerServer : boost::noncopyable 00057 { 00058 public: 00059 00060 typedef visualization_msgs::InteractiveMarkerFeedbackConstPtr FeedbackConstPtr; 00061 typedef boost::function< void ( const FeedbackConstPtr& ) > FeedbackCallback; 00062 00063 static const uint8_t DEFAULT_FEEDBACK_CB = 255; 00064 00072 InteractiveMarkerServer( const std::string &topic_ns, const std::string &server_id="", bool spin_thread = false ); 00073 00075 ~InteractiveMarkerServer(); 00076 00081 void insert( const visualization_msgs::InteractiveMarker &int_marker ); 00082 00089 void insert( const visualization_msgs::InteractiveMarker &int_marker, 00090 FeedbackCallback feedback_cb, 00091 uint8_t feedback_type=DEFAULT_FEEDBACK_CB ); 00092 00099 bool setPose( const std::string &name, 00100 const geometry_msgs::Pose &pose, 00101 const std_msgs::Header &header=std_msgs::Header() ); 00102 00107 bool erase( const std::string &name ); 00108 00111 void clear(); 00112 00123 bool setCallback( const std::string &name, FeedbackCallback feedback_cb, 00124 uint8_t feedback_type=DEFAULT_FEEDBACK_CB ); 00125 00128 void applyChanges(); 00129 00134 bool get( std::string name, visualization_msgs::InteractiveMarker &int_marker ) const; 00135 00136 private: 00137 00138 struct MarkerContext 00139 { 00140 ros::Time last_feedback; 00141 std::string last_client_id; 00142 FeedbackCallback default_feedback_cb; 00143 boost::unordered_map<uint8_t,FeedbackCallback> feedback_cbs; 00144 visualization_msgs::InteractiveMarker int_marker; 00145 }; 00146 00147 typedef boost::unordered_map< std::string, MarkerContext > M_MarkerContext; 00148 00149 // represents an update to a single marker 00150 struct UpdateContext 00151 { 00152 enum { 00153 FULL_UPDATE, 00154 POSE_UPDATE, 00155 ERASE 00156 } update_type; 00157 visualization_msgs::InteractiveMarker int_marker; 00158 FeedbackCallback default_feedback_cb; 00159 boost::unordered_map<uint8_t,FeedbackCallback> feedback_cbs; 00160 }; 00161 00162 typedef boost::unordered_map< std::string, UpdateContext > M_UpdateContext; 00163 00164 // main loop when spinning our own thread 00165 // - process callbacks in our callback queue 00166 // - process pending goals 00167 void spinThread(); 00168 00169 // update marker pose & call user callback 00170 void processFeedback( const FeedbackConstPtr& feedback ); 00171 00172 // send an empty update to keep the client GUIs happy 00173 void keepAlive(); 00174 00175 // increase sequence number & publish an update 00176 void publish( visualization_msgs::InteractiveMarkerUpdate &update ); 00177 00178 // publish the current complete state to the latched "init" topic. 00179 void publishInit(); 00180 00181 // Update pose, schedule update without locking 00182 void doSetPose( M_UpdateContext::iterator update_it, 00183 const std::string &name, 00184 const geometry_msgs::Pose &pose, 00185 const std_msgs::Header &header ); 00186 00187 // contains the current state of all markers 00188 M_MarkerContext marker_contexts_; 00189 00190 // updates that have to be sent on the next publish 00191 M_UpdateContext pending_updates_; 00192 00193 // topic namespace to use 00194 std::string topic_ns_; 00195 00196 boost::recursive_mutex mutex_; 00197 00198 // these are needed when spinning up a dedicated thread 00199 boost::scoped_ptr<boost::thread> spin_thread_; 00200 ros::NodeHandle node_handle_; 00201 ros::CallbackQueue callback_queue_; 00202 volatile bool need_to_terminate_; 00203 00204 // this is needed when running in non-threaded mode 00205 ros::Timer keep_alive_timer_; 00206 00207 ros::Publisher init_pub_; 00208 ros::Publisher update_pub_; 00209 ros::Subscriber feedback_sub_; 00210 00211 uint64_t seq_num_; 00212 00213 std::string server_id_; 00214 }; 00215 00216 } 00217 00218 #endif