34 #include <visualization_msgs/InteractiveMarkerInit.h> 36 #include <boost/bind.hpp> 37 #include <boost/make_shared.hpp> 52 if (!server_id.empty())
61 std::string update_topic = topic_ns +
"/update";
62 std::string init_topic = update_topic +
"_full";
63 std::string feedback_topic = topic_ns +
"/feedback";
112 boost::recursive_mutex::scoped_lock lock(
mutex_ );
119 M_UpdateContext::iterator update_it;
121 visualization_msgs::InteractiveMarkerUpdate
update;
122 update.type = visualization_msgs::InteractiveMarkerUpdate::UPDATE;
130 M_MarkerContext::iterator marker_context_it =
marker_contexts_.find( update_it->first );
132 switch ( update_it->second.update_type )
138 ROS_DEBUG(
"Creating new context for %s", update_it->first.c_str());
142 marker_context_it->second.default_feedback_cb = update_it->second.default_feedback_cb;
143 marker_context_it->second.feedback_cbs = update_it->second.feedback_cbs;
146 marker_context_it->second.int_marker = update_it->second.int_marker;
148 update.markers.push_back( marker_context_it->second.int_marker );
156 ROS_ERROR(
"Pending pose update for non-existing marker found. This is a bug in InteractiveMarkerInterface." );
160 marker_context_it->second.int_marker.pose = update_it->second.int_marker.pose;
161 marker_context_it->second.int_marker.header = update_it->second.int_marker.header;
163 visualization_msgs::InteractiveMarkerPose pose_update;
164 pose_update.header = marker_context_it->second.int_marker.header;
165 pose_update.pose = marker_context_it->second.int_marker.pose;
166 pose_update.name = marker_context_it->second.int_marker.name;
167 update.poses.push_back( pose_update );
177 update.erases.push_back( update_it->first );
194 boost::recursive_mutex::scoped_lock lock(
mutex_ );
207 boost::recursive_mutex::scoped_lock lock(
mutex_ );
211 M_MarkerContext::iterator it;
233 boost::recursive_mutex::scoped_lock lock(
mutex_ );
235 M_MarkerContext::iterator marker_context_it =
marker_contexts_.find( name );
246 if ( header.frame_id.empty() )
250 doSetPose( update_it, name, pose, marker_context_it->second.int_marker.header );
254 doSetPose( update_it, name, pose, update_it->second.int_marker.header );
258 BOOST_ASSERT_MSG(
false,
"Marker does not exist and there is no pending creation.");
264 doSetPose( update_it, name, pose, header );
271 boost::recursive_mutex::scoped_lock lock(
mutex_ );
273 M_MarkerContext::iterator marker_context_it =
marker_contexts_.find( name );
289 marker_context_it->second.default_feedback_cb = feedback_cb;
295 marker_context_it->second.feedback_cbs[feedback_type] = feedback_cb;
299 marker_context_it->second.feedback_cbs.erase( feedback_type );
308 update_it->second.default_feedback_cb = feedback_cb;
314 update_it->second.feedback_cbs[feedback_type] = feedback_cb;
318 update_it->second.feedback_cbs.erase( feedback_type );
327 boost::recursive_mutex::scoped_lock lock(
mutex_ );
329 M_UpdateContext::iterator update_it =
pending_updates_.find( int_marker.name );
336 update_it->second.int_marker = int_marker;
344 setCallback( int_marker.name, feedback_cb, feedback_type );
349 boost::recursive_mutex::scoped_lock lock(
mutex_ );
355 M_MarkerContext::const_iterator marker_context_it =
marker_contexts_.find( name );
361 int_marker = marker_context_it->second.int_marker;
366 switch ( update_it->second.update_type )
373 M_MarkerContext::const_iterator marker_context_it =
marker_contexts_.find( name );
378 int_marker = marker_context_it->second.int_marker;
379 int_marker.pose = update_it->second.int_marker.pose;
384 int_marker = update_it->second.int_marker;
393 boost::recursive_mutex::scoped_lock lock(
mutex_ );
395 visualization_msgs::InteractiveMarkerInit init;
400 M_MarkerContext::iterator it;
403 ROS_DEBUG(
"Publishing %s", it->second.int_marker.name.c_str() );
404 init.markers.push_back( it->second.int_marker );
412 boost::recursive_mutex::scoped_lock lock(
mutex_ );
414 M_MarkerContext::iterator marker_context_it =
marker_contexts_.find( feedback->marker_name );
428 ROS_DEBUG(
"Rejecting feedback for %s: conflicting feedback from separate clients.", feedback->marker_name.c_str() );
435 if ( feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE )
449 boost::unordered_map<uint8_t,FeedbackCallback>::iterator feedback_cb_it = marker_context.
feedback_cbs.find( feedback->event_type );
450 if ( feedback_cb_it != marker_context.
feedback_cbs.end() && feedback_cb_it->second )
453 feedback_cb_it->second( feedback );
465 visualization_msgs::InteractiveMarkerUpdate empty_update;
466 empty_update.type = visualization_msgs::InteractiveMarkerUpdate::KEEP_ALIVE;
491 update_it->second.int_marker.pose = pose;
492 update_it->second.int_marker.header = header;
493 ROS_DEBUG(
"Marker '%s' is now at %f, %f, %f", update_it->first.c_str(), pose.position.x, pose.position.y, pose.position.z );
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
visualization_msgs::InteractiveMarkerFeedbackConstPtr FeedbackConstPtr
boost::unordered_map< uint8_t, FeedbackCallback > feedback_cbs
ros::Subscriber feedback_sub_
void processFeedback(const FeedbackConstPtr &feedback)
ROSCPP_DECL const std::string & getName()
visualization_msgs::InteractiveMarker int_marker
boost::function< void(const FeedbackConstPtr &) > FeedbackCallback
M_MarkerContext marker_contexts_
static const uint8_t DEFAULT_FEEDBACK_CB
FeedbackCallback default_feedback_cb
INTERACTIVE_MARKERS_PUBLIC ~InteractiveMarkerServer()
Destruction of the interface will lead to all managed markers being cleared.
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
ros::Timer keep_alive_timer_
ros::NodeHandle node_handle_
void setCallbackQueue(CallbackQueueInterface *queue)
INTERACTIVE_MARKERS_PUBLIC InteractiveMarkerServer(const std::string &topic_ns, const std::string &server_id="", bool spin_thread=false)
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
volatile bool need_to_terminate_
ros::CallbackQueue callback_queue_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
INTERACTIVE_MARKERS_PUBLIC bool get(std::string name, visualization_msgs::InteractiveMarker &int_marker) const
void doSetPose(M_UpdateContext::iterator update_it, const std::string &name, const geometry_msgs::Pose &pose, const std_msgs::Header &header)
boost::recursive_mutex mutex_
M_UpdateContext pending_updates_
INTERACTIVE_MARKERS_PUBLIC void insert(const visualization_msgs::InteractiveMarker &int_marker)
void publish(visualization_msgs::InteractiveMarkerUpdate &update)
INTERACTIVE_MARKERS_PUBLIC void applyChanges()
INTERACTIVE_MARKERS_PUBLIC bool setCallback(const std::string &name, FeedbackCallback feedback_cb, uint8_t feedback_type=DEFAULT_FEEDBACK_CB)
INTERACTIVE_MARKERS_PUBLIC void clear()
INTERACTIVE_MARKERS_PUBLIC bool setPose(const std::string &name, const geometry_msgs::Pose &pose, const std_msgs::Header &header=std_msgs::Header())
boost::scoped_ptr< boost::thread > spin_thread_
std::string last_client_id
ros::Publisher update_pub_
INTERACTIVE_MARKERS_PUBLIC bool erase(const std::string &name)
INTERACTIVE_MARKERS_PUBLIC bool empty() const
INTERACTIVE_MARKERS_PUBLIC std::size_t size() const