proxy.cpp
Go to the documentation of this file.
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 
00030 #include <ros/ros.h>
00031 #include <ros/console.h>
00032 
00033 #include <tf/transform_listener.h>
00034 
00035 #include <interactive_markers/interactive_marker_client.h>
00036 #include <interactive_marker_proxy/GetInit.h>
00037 
00038 using namespace interactive_markers;
00039 
00040 class Proxy
00041 {
00042 public:
00043   ros::NodeHandle nh_;
00044   ros::Publisher pub_;
00045   tf::TransformListener tf_;
00046   interactive_markers::InteractiveMarkerClient client_;
00047   std::string topic_ns_;
00048   std::string target_frame_;
00049   ros::ServiceServer service_;
00050   std::map<std::string, std::string> status_text_;
00051   ros::Timer timer_;
00052 
00053   std::map<std::string, visualization_msgs::InteractiveMarker> int_markers_;
00054 
00055   Proxy(std::string target_frame, std::string topic_ns) :
00056       client_(tf_, target_frame, topic_ns), topic_ns_(topic_ns), target_frame_(target_frame)
00057   {
00058     ROS_INFO_STREAM("Subscribing to " << topic_ns);
00059     ROS_INFO_STREAM("Target frame set to " << target_frame);
00060 
00061     client_.setInitCb(boost::bind(&Proxy::initCb, this, _1));
00062     client_.setUpdateCb(boost::bind(&Proxy::updateCb, this, _1));
00063     client_.setResetCb(boost::bind(&Proxy::resetCb, this, _1));
00064     client_.setStatusCb(boost::bind(&Proxy::statusCb, this, _1, _2, _3));
00065     client_.subscribe(topic_ns_);
00066 
00067     pub_ = nh_.advertise<visualization_msgs::InteractiveMarkerUpdate>(topic_ns_ + "/tunneled/update", 1000);
00068 
00069     service_ = nh_.advertiseService(topic_ns_ + "/tunneled/get_init", &Proxy::getInit, this);
00070 
00071     ros::NodeHandle private_nh("~");
00072     double update_rate;
00073     private_nh.param<double>("update_rate", update_rate, 30.0f);
00074     timer_ = nh_.createTimer(ros::Duration(1.0 / update_rate), boost::bind(&Proxy::timerCb, this, _1));
00075   }
00076 
00077   typedef visualization_msgs::InteractiveMarkerInitConstPtr InitConstPtr;
00078   typedef visualization_msgs::InteractiveMarkerUpdateConstPtr UpdateConstPtr;
00079   typedef visualization_msgs::InteractiveMarkerUpdatePtr UpdatePtr;
00080 
00081   void timerCb(const ros::TimerEvent&)
00082   {
00083     client_.update();
00084   }
00085 
00086   bool getInit(interactive_marker_proxy::GetInit::Request& request,
00087                interactive_marker_proxy::GetInit::Response& response)
00088   {
00089     ROS_INFO("Init requested.");
00090     std::vector< visualization_msgs::InteractiveMarker > markers;
00091     std::map<std::string, visualization_msgs::InteractiveMarker>::iterator it;
00092     for( it = int_markers_.begin(); it!=int_markers_.end(); it++ )
00093     {
00094       response.msg.markers.push_back(it->second);
00095     }
00096     return true;
00097   }
00098 
00099   void updateCb(const UpdateConstPtr& up_msg)
00100   {
00101     const visualization_msgs::InteractiveMarkerUpdate::_erases_type& erases = up_msg->erases;
00102     for (unsigned i = 0; i < erases.size(); i++)
00103     {
00104       int_markers_.erase(erases[i]);
00105     }
00106 
00107     const visualization_msgs::InteractiveMarkerUpdate::_poses_type& poses = up_msg->poses;
00108     for (unsigned i = 0; i < poses.size(); i++)
00109     {
00110       int_markers_[poses[i].name].pose = poses[i].pose;
00111       int_markers_[poses[i].name].header = poses[i].header;
00112     }
00113 
00114     const visualization_msgs::InteractiveMarkerUpdate::_markers_type& markers = up_msg->markers;
00115     for (unsigned i = 0; i < markers.size(); i++)
00116     {
00117       int_markers_[markers[i].name] = markers[i];
00118     }
00119 
00120     pub_.publish(up_msg);
00121   }
00122 
00123   void initCb(const InitConstPtr& init_msg)
00124   {
00125     UpdatePtr update(new visualization_msgs::InteractiveMarkerUpdate());
00126     update->markers = init_msg->markers;
00127     update->seq_num = init_msg->seq_num;
00128     update->server_id = init_msg->server_id;
00129 
00130     int_markers_.clear();
00131     updateCb(update);
00132   }
00133 
00134   void statusCb(InteractiveMarkerClient::StatusT status, const std::string& server_id, const std::string& status_text)
00135   {
00136     if ( status_text_.find(server_id) != status_text_.end() &&
00137          status_text_[server_id] == status_text )
00138     {
00139       return;
00140     }
00141     status_text_[server_id] = status_text;
00142     std::string status_string[] = {"INFO", "WARN", "ERROR"};
00143     ROS_INFO_STREAM( "(" << status_string[(unsigned)status] << ") " << server_id << ": " << status_text);
00144   }
00145 
00146   void resetCb(const std::string& server_id)
00147   {
00148   }
00149 };
00150 
00151 int main(int argc, char **argv)
00152 {
00153   ros::init(argc, argv, "interactive_marker_proxy");
00154   {
00155     ros::NodeHandle nh;
00156     Proxy proxy(nh.resolveName("target_frame"), nh.resolveName("topic_ns"));
00157     ros::spin();
00158   }
00159 }


interactive_marker_proxy
Author(s): David Gossow
autogenerated on Mon Oct 6 2014 00:55:27