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 #ifndef INTERACTIVE_MARKER_CLIENT_H 00030 #define INTERACTIVE_MARKER_CLIENT_H 00031 00032 #include <visualization_msgs/InteractiveMarkerUpdate.h> 00033 #include <visualization_msgs/InteractiveMarkerInit.h> 00034 00035 namespace rviz 00036 { 00037 00043 class InteractiveMarkerReceiver 00044 { 00045 public: 00051 virtual void processMarkerChanges( const std::vector<visualization_msgs::InteractiveMarker>* markers = NULL, 00052 const std::vector<visualization_msgs::InteractiveMarkerPose>* poses = NULL, 00053 const std::vector<std::string>* erases = NULL ) = 0; 00057 virtual void clearMarkers() = 0; 00058 00063 virtual bool subscribeToInit() = 0; 00064 00069 virtual void unsubscribeFromInit() = 0; 00070 00075 virtual void setStatusOk(const std::string& name, const std::string& text) = 0; 00076 00081 virtual void setStatusWarn(const std::string& name, const std::string& text) = 0; 00082 00087 virtual void setStatusError(const std::string& name, const std::string& text) = 0; 00088 }; 00089 00100 class InteractiveMarkerClient 00101 { 00102 public: 00107 InteractiveMarkerClient( InteractiveMarkerReceiver* receiver ); 00108 00113 void processMarkerUpdate(const visualization_msgs::InteractiveMarkerUpdate::ConstPtr& marker_update); 00114 00119 void processMarkerInit(const visualization_msgs::InteractiveMarkerInit::ConstPtr& marker_init); 00120 00126 void reinit(); 00127 00133 void unsubscribedFromInit(); 00134 00138 bool isPublisherListEmpty(); 00139 00143 void clear(); 00144 00150 void flagLateConnections(); 00151 00152 private: 00153 00154 InteractiveMarkerReceiver* receiver_; 00155 00156 typedef std::map<uint64_t, visualization_msgs::InteractiveMarkerUpdate::ConstPtr> M_InteractiveMarkerUpdate; 00157 00158 struct PublisherContext { 00159 bool update_seen; 00160 bool init_seen; 00161 uint64_t last_update_seq_num; 00162 uint64_t last_init_seq_num; 00163 ros::Time last_update_time; 00164 bool update_time_ok; 00165 bool initialized; 00166 00167 // We queue up UPDATE messages which arrive before we get an init 00168 // message, in case the update messages are ahead of the init 00169 // messages. No KEEP_ALIVE messages are stored here. 00170 M_InteractiveMarkerUpdate update_queue; 00171 00172 PublisherContext(); 00173 void enqueueUpdate(const visualization_msgs::InteractiveMarkerUpdate::ConstPtr& update); 00174 00175 // Update the value of initialized based on previous sequence 00176 // numbers and the number from this new update. 00177 bool checkInitWith(const visualization_msgs::InteractiveMarkerUpdate::ConstPtr& update); 00178 00179 // Update the value of initialized based on previous sequence 00180 // numbers and the number from this init message. 00181 bool checkInitWith(const visualization_msgs::InteractiveMarkerInit::ConstPtr& init); 00182 }; 00183 typedef boost::shared_ptr<PublisherContext> PublisherContextPtr; 00184 00185 typedef std::map<std::string, PublisherContextPtr> M_PublisherContext; 00186 M_PublisherContext publisher_contexts_; 00187 00188 // If the markers have been cleared since the last call to 00189 // InteractiveMarkerReceiver::processMarkerChanges(). 00190 bool cleared_; 00191 00192 // If we have called InteractiveMarkerReceiver::subscribeToInit() 00193 // since we've called 00194 // InteractiveMarkerReceiver::unsubscribeFromInit(). 00195 bool subscribed_to_init_; 00196 00197 // play back the relevant updates from the queue, if any, and clear the queue. 00198 void playbackUpdateQueue( PublisherContextPtr& context ); 00199 00200 // Apply a single update message from a given publisher context. 00201 void applyUpdate( const visualization_msgs::InteractiveMarkerUpdate::ConstPtr& marker_update, 00202 PublisherContextPtr& context ); 00203 00204 // Check if we are ready to unsubscribe from the init topic, and 00205 // unsubscribe if so. 00206 void maybeUnsubscribeFromInit(); 00207 }; 00208 00209 } // namespace rviz 00210 00211 #endif // INTERACTIVE_MARKER_CLIENT_H