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 <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;

  InteractiveMarkerServer( const std::string &topic_ns, const std::string &server_id="", bool spin_thread = false );

  ~InteractiveMarkerServer();

  void insert( const visualization_msgs::InteractiveMarker &int_marker );

  void insert( const visualization_msgs::InteractiveMarker &int_marker,
               FeedbackCallback feedback_cb,
               uint8_t feedback_type=DEFAULT_FEEDBACK_CB );

  bool setPose( const std::string &name,
      const geometry_msgs::Pose &pose,
      const std_msgs::Header &header=std_msgs::Header() );

  bool erase( const std::string &name );

  void clear();

  bool setCallback( const std::string &name, FeedbackCallback feedback_cb,
      uint8_t feedback_type=DEFAULT_FEEDBACK_CB );

  void applyChanges();

  bool get( std::string name, visualization_msgs::InteractiveMarker &int_marker ) const;

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_;
  
  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