interactive_marker_server.h

Go to the documentation of this file.

/*
 * Copyright (c) 2011, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 * 
 * Author: David Gossow
 */
 
#ifndef INTERACTIVE_MARKER_SERVER
#define INTERACTIVE_MARKER_SERVER
 
#include <visualization_msgs/InteractiveMarkerUpdate.h>
#include <visualization_msgs/InteractiveMarkerFeedback.h>
#include <interactive_markers/visibility_control.hpp>
 
#include <boost/scoped_ptr.hpp>
#include <boost/thread/thread.hpp>
#include <boost/thread/recursive_mutex.hpp>
 
#include <ros/ros.h>
#include <ros/callback_queue.h>
 
#include <boost/function.hpp>
#include <boost/unordered_map.hpp>
 
namespace interactive_markers
{
 
class InteractiveMarkerServer : boost::noncopyable
{
public:
 
  typedef visualization_msgs::InteractiveMarkerFeedbackConstPtr FeedbackConstPtr;
  typedef boost::function< void ( const FeedbackConstPtr& ) > FeedbackCallback;
 
  static const uint8_t DEFAULT_FEEDBACK_CB = 255;
 
  INTERACTIVE_MARKERS_PUBLIC
  InteractiveMarkerServer( const std::string &topic_ns, const std::string &server_id="", bool spin_thread = false );
 
  INTERACTIVE_MARKERS_PUBLIC
  ~InteractiveMarkerServer();
 
  INTERACTIVE_MARKERS_PUBLIC
  void insert( const visualization_msgs::InteractiveMarker &int_marker );
 
  INTERACTIVE_MARKERS_PUBLIC
  void insert( const visualization_msgs::InteractiveMarker &int_marker,
               FeedbackCallback feedback_cb,
               uint8_t feedback_type=DEFAULT_FEEDBACK_CB );
 
  INTERACTIVE_MARKERS_PUBLIC
  bool setPose( const std::string &name,
      const geometry_msgs::Pose &pose,
      const std_msgs::Header &header=std_msgs::Header() );
 
  INTERACTIVE_MARKERS_PUBLIC
  bool erase( const std::string &name );
 
  INTERACTIVE_MARKERS_PUBLIC
  void clear();
  
  INTERACTIVE_MARKERS_PUBLIC
  bool empty() const;
  
  INTERACTIVE_MARKERS_PUBLIC
  std::size_t size() const;
 
  INTERACTIVE_MARKERS_PUBLIC
  bool setCallback( const std::string &name, FeedbackCallback feedback_cb,
      uint8_t feedback_type=DEFAULT_FEEDBACK_CB );
 
  INTERACTIVE_MARKERS_PUBLIC
  void applyChanges();
 
  INTERACTIVE_MARKERS_PUBLIC
  bool get( std::string name, visualization_msgs::InteractiveMarker &int_marker ) const;
 
  INTERACTIVE_MARKERS_PUBLIC
  std::vector<std::string> getNames();
 
private:
 
  struct MarkerContext
  {
    ros::Time last_feedback;
    std::string last_client_id;
    FeedbackCallback default_feedback_cb;
    boost::unordered_map<uint8_t,FeedbackCallback> feedback_cbs;
    visualization_msgs::InteractiveMarker int_marker;
  };
 
  typedef boost::unordered_map< std::string, MarkerContext > M_MarkerContext;
 
  // represents an update to a single marker
  struct UpdateContext
  {
    enum {
      FULL_UPDATE,
      POSE_UPDATE,
      ERASE
    } update_type;
    visualization_msgs::InteractiveMarker int_marker;
    FeedbackCallback default_feedback_cb;
    boost::unordered_map<uint8_t,FeedbackCallback> feedback_cbs;
  };
 
  typedef boost::unordered_map< std::string, UpdateContext > M_UpdateContext;
 
  // main loop when spinning our own thread
  // - process callbacks in our callback queue
  // - process pending goals
  void spinThread();
 
  // update marker pose & call user callback
  void processFeedback( const FeedbackConstPtr& feedback );
 
  // send an empty update to keep the client GUIs happy
  void keepAlive();
 
  // increase sequence number & publish an update
  void publish( visualization_msgs::InteractiveMarkerUpdate &update );
 
  // publish the current complete state to the latched "init" topic.
  void publishInit();
 
  // Update pose, schedule update without locking
  void doSetPose( M_UpdateContext::iterator update_it,
      const std::string &name,
      const geometry_msgs::Pose &pose,
      const std_msgs::Header &header );
 
  // contains the current state of all markers
  M_MarkerContext marker_contexts_;
 
  // updates that have to be sent on the next publish
  M_UpdateContext pending_updates_;
 
  // topic namespace to use
  std::string topic_ns_;
  
  mutable boost::recursive_mutex mutex_;
 
  // these are needed when spinning up a dedicated thread
  boost::scoped_ptr<boost::thread> spin_thread_;
  ros::NodeHandle node_handle_;
  ros::CallbackQueue callback_queue_;
  volatile bool need_to_terminate_;
 
  // this is needed when running in non-threaded mode
  ros::Timer keep_alive_timer_;
 
  ros::Publisher init_pub_;
  ros::Publisher update_pub_;
  ros::Subscriber feedback_sub_;
 
  uint64_t seq_num_;
 
  std::string server_id_;
};
 
}
 
#endif