34 #include <boost/bind.hpp> 35 #include <boost/make_shared.hpp> 37 #define DBG_MSG( ... ) ROS_DEBUG( __VA_ARGS__ ); 44 const std::string& server_id,
49 : state_(server_id,INIT)
50 , first_update_seq_num_(-1)
51 , last_update_seq_num_(-1)
53 , target_frame_(target_frame)
54 , callbacks_(callbacks)
55 , server_id_(server_id)
56 , warn_keepalive_(false)
66 void SingleClient::process(
const visualization_msgs::InteractiveMarkerInit::ConstPtr& msg,
bool enable_autocomplete_transparency)
75 DBG_MSG(
"Init queue too large. Erasing init message with id %lu.",
init_queue_.begin()->msg->seq_num );
88 void SingleClient::process(
const visualization_msgs::InteractiveMarkerUpdate::ConstPtr& msg,
bool enable_autocomplete_transparency)
97 if ( msg->type == msg->KEEP_ALIVE )
102 std::ostringstream
s;
103 s <<
"Sequence number of update is out of order. Expected: " <<
last_update_seq_num_ <<
" Received: " << msg->seq_num;
115 std::ostringstream
s;
116 s <<
"Sequence number of update is out of order. Expected: " <<
last_update_seq_num_+1 <<
" Received: " << msg->seq_num;
128 DBG_MSG(
"Update queue too large. Erasing update message with id %lu.",
update_queue_.begin()->msg->seq_num );
159 errorReset(
"Update queue overflow. Resetting connection." );
164 if (
state_.getDuration().toSec() > 1.0 )
176 if ( time_since_upd > 2.0 )
178 std::ostringstream
s;
179 s <<
"No update received for " << round(time_since_upd) <<
" seconds.";
203 M_InitMessageContext::iterator init_it;
206 uint64_t init_seq_num = init_it->msg->seq_num;
209 if ( !init_it->isReady() )
214 else if ( next_up_exists )
216 DBG_MSG(
"Init message with seq_id=%lu is ready & in line with updates. Switching to receive mode.", init_seq_num );
237 M_InitMessageContext::iterator it;
242 it->getTfTransforms();
244 catch ( std::runtime_error& e )
248 std::ostringstream
s;
249 s <<
"Cannot get tf info for init message with sequence number " << it->msg->seq_num <<
". Error: " << e.what();
258 M_UpdateMessageContext::iterator it;
263 it->getTfTransforms();
265 catch ( std::runtime_error& e )
267 std::ostringstream
s;
268 s <<
"Resetting due to tf error: " << e.what();
274 std::ostringstream
s;
275 s <<
"Resetting due to unknown exception";
MessageContext< visualization_msgs::InteractiveMarkerInit > InitMessageContext
MessageContext< visualization_msgs::InteractiveMarkerUpdate > UpdateMessageContext
M_UpdateMessageContext update_queue_
void updateCb(const UpdateConstPtr &u) const
ros::Time last_update_time_
M_InitMessageContext init_queue_
void initCb(const InitConstPtr &i) const
void errorReset(std::string error_msg)
void process(const visualization_msgs::InteractiveMarkerUpdate::ConstPtr &msg, bool enable_autocomplete_transparency=true)
void transformUpdateMsgs()
const InteractiveMarkerClient::CbCollection & callbacks_
StateMachine< StateT > state_
uint64_t first_update_seq_num_
void resetCb(const std::string &s) const
std::string target_frame_
SingleClient(const std::string &server_id, tf::Transformer &tf, const std::string &target_frame, const InteractiveMarkerClient::CbCollection &callbacks)
void statusCb(StatusT s, const std::string &id, const std::string &m) const
uint64_t last_update_seq_num_