diff_subscriber.cpp
Go to the documentation of this file.00001 /* 00002 * Copyright (c) 2008, 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 00039 #include <pose_graph/diff_subscriber.h> 00040 #include <graph_mapping_msgs/GetGraph.h> 00041 #include <pose_graph/message_conversion.h> 00042 00043 namespace pose_graph 00044 { 00045 00046 namespace msg=graph_mapping_msgs; 00047 00048 00049 DiffSubscriber::DiffSubscriber (const DiffSubscriber::DiffCallback& cb) : 00050 diff_callback_(cb), graph_client_(nh_.serviceClient<msg::GetGraph>("get_full_graph")), last_id_(0) 00051 { 00052 // We don't want to wait till the first diff is sent. So we invoke our own callback 00053 // with a fake diff to trigger a service call. Note that it's fine if 00054 00055 msg::ConstraintGraphDiff fake_diff; 00056 fake_diff.id = 42; // Anything but 1 00057 diffCB(fake_diff); 00058 00059 // Now that the current graph is received, subscribe to the true diff topic 00060 diff_sub_ = nh_.subscribe("graph_diffs", 10, &DiffSubscriber::diffCB, this); 00061 } 00062 00063 00064 void DiffSubscriber::diffCB (const msg::ConstraintGraphDiff& diff) 00065 { 00066 boost::optional<const msg::ConstraintGraphDiff&> possible_diff; 00067 { 00068 boost::mutex::scoped_lock l(mutex_); 00069 if (diff.id == last_id_+1) { 00070 applyDiff(&graph_, diff); 00071 possible_diff = diff; 00072 last_id_ = diff.id; 00073 } 00074 else { 00075 ROS_INFO_STREAM ("Received diff with id " << diff.id << " while last id is " << last_id_); 00076 msg::GetGraph srv; 00077 graph_client_.call(srv); 00078 graph_ = constraintGraphFromMessage(srv.response.graph); 00079 last_id_ = srv.response.id; 00080 ROS_INFO_STREAM ("New graph has id " << last_id_); 00081 } 00082 } 00083 // Not giving up the lock allows deadlock if the callback acquires a lock 00084 // that also surrounds other ops on this class. It's ok to do this because 00085 // 1) only one instance of diffCB can be running at a time, and that's the only 00086 // non-const member function 2) graph_ is const for diff_callback_ 00087 00088 diff_callback_(possible_diff, graph_); 00089 } 00090 00091 ConstraintGraph DiffSubscriber::getCurrentGraph () const 00092 { 00093 boost::mutex::scoped_lock l(mutex_); 00094 return graph_; 00095 } 00096 00097 00098 00099 00100 00101 } // namespace