00001 /* 00002 * Copyright (c) 2012, 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 * single_client.h 00032 * 00033 * Created on: Jul 17, 2012 00034 * Author: gossow 00035 */ 00036 00037 #ifndef SINGLE_CLIENT_H_ 00038 #define SINGLE_CLIENT_H_ 00039 00040 #include <visualization_msgs/InteractiveMarkerInit.h> 00041 #include <visualization_msgs/InteractiveMarkerUpdate.h> 00042 00043 #include <boost/scoped_ptr.hpp> 00044 #include <boost/thread/thread.hpp> 00045 #include <boost/thread/recursive_mutex.hpp> 00046 00047 #include <boost/function.hpp> 00048 #include <boost/unordered_map.hpp> 00049 00050 #include <tf/tf.h> 00051 00052 #include <deque> 00053 00054 #include "message_context.h" 00055 #include "state_machine.h" 00056 #include "../interactive_marker_client.h" 00057 00058 00059 namespace interactive_markers 00060 { 00061 00062 class SingleClient 00063 { 00064 public: 00065 00066 SingleClient( 00067 const std::string& server_id, 00068 tf::Transformer& tf, 00069 const std::string& target_frame, 00070 const InteractiveMarkerClient::CbCollection& callbacks ); 00071 00072 ~SingleClient(); 00073 00074 // Process message from the update channel 00075 void process(const visualization_msgs::InteractiveMarkerUpdate::ConstPtr& msg, bool enable_autocomplete_transparency = true); 00076 00077 // Process message from the init channel 00078 void process(const visualization_msgs::InteractiveMarkerInit::ConstPtr& msg, bool enable_autocomplete_transparency = true); 00079 00080 // true if INIT messages are not needed anymore 00081 bool isInitialized(); 00082 00083 // transform all messages with missing transforms 00084 void update(); 00085 00086 private: 00087 00088 // check if we can go from init state to normal operation 00089 void checkInitFinished(); 00090 00091 void checkKeepAlive(); 00092 00093 enum StateT 00094 { 00095 INIT, 00096 RECEIVING, 00097 TF_ERROR 00098 }; 00099 00100 StateMachine<StateT> state_; 00101 00102 // updateTf implementation (for one queue) 00103 void transformInitMsgs( ); 00104 void transformUpdateMsgs( ); 00105 00106 void pushUpdates(); 00107 00108 void errorReset( std::string error_msg ); 00109 00110 // sequence number and time of first ever received update 00111 uint64_t first_update_seq_num_; 00112 00113 // sequence number and time of last received update 00114 uint64_t last_update_seq_num_; 00115 ros::Time last_update_time_; 00116 00117 // true if the last outgoing update is too long ago 00118 // and we've already sent a notification of that 00119 bool update_time_ok_; 00120 00121 typedef MessageContext<visualization_msgs::InteractiveMarkerUpdate> UpdateMessageContext; 00122 typedef MessageContext<visualization_msgs::InteractiveMarkerInit> InitMessageContext; 00123 00124 // Queue of Updates waiting for tf and numbering 00125 typedef std::deque< UpdateMessageContext > M_UpdateMessageContext; 00126 typedef std::deque< InitMessageContext > M_InitMessageContext; 00127 00128 // queue for update messages 00129 M_UpdateMessageContext update_queue_; 00130 00131 // queue for init messages 00132 M_InitMessageContext init_queue_; 00133 00134 tf::Transformer& tf_; 00135 std::string target_frame_; 00136 00137 const InteractiveMarkerClient::CbCollection& callbacks_; 00138 00139 std::string server_id_; 00140 00141 bool warn_keepalive_; 00142 }; 00143 00144 } 00145 00146 #endif /* SINGLE_CLIENT_H_ */