route.cpp
Go to the documentation of this file.
00001 // *****************************************************************************
00002 //
00003 // Copyright (c) 2016, Southwest Research Institute® (SwRI®)
00004 // All rights reserved.
00005 //
00006 // Redistribution and use in source and binary forms, with or without
00007 // modification, are permitted provided that the following conditions are met:
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 Southwest Research Institute® (SwRI®) nor the
00014 //       names of its contributors may be used to endorse or promote products
00015 //       derived from 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 <COPYRIGHT HOLDER> BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //
00028 // *****************************************************************************
00029 #include <swri_route_util/route.h>
00030 #include <swri_route_util/route_point.h>
00031 #include <boost/make_shared.hpp>
00032 
00033 namespace mnm = marti_nav_msgs;
00034 
00035 namespace swri_route_util
00036 {
00037 Route::Route()
00038 {
00039 }
00040 
00041 // Helper function used by the Route(mnm::Route) constructor.
00042 static
00043 void pointFromMsg(RoutePoint &dst, const marti_nav_msgs::RoutePoint &src)
00044 {
00045   dst.setPose(src.pose);
00046   dst.setId(src.id);
00047 
00048   for (auto const &prop : src.properties) {
00049     dst.setProperty(prop.key, prop.value);
00050   }
00051 }
00052 
00053 Route::Route(const mnm::Route &msg)
00054 {
00055   header = msg.header;
00056 
00057   points.resize(msg.route_points.size());
00058   for (size_t i = 0; i < points.size(); ++i) {
00059     pointFromMsg(points[i], msg.route_points[i]);
00060   }
00061 
00062   for (auto const &prop : msg.properties) {
00063     setProperty(prop.key, prop.value);
00064   }
00065 
00066   rebuildPointIndex();
00067 }
00068 
00069 // Helper method used by the toMsg() method.
00070 static
00071 void msgFromPoint(marti_nav_msgs::RoutePoint &dst, const RoutePoint &src)
00072 {
00073   dst.pose = src.poseMsg();
00074   dst.id = src.id();
00075   
00076   std::vector<std::string> names = src.getPropertyNames();
00077   dst.properties.resize(names.size());
00078   for (size_t i = 0; i < names.size(); ++i) {
00079     dst.properties[i].key = names[i];
00080     dst.properties[i].value = src.getProperty(names[i]);
00081   }
00082 }
00083 
00084 void Route::toMsg(mnm::Route &msg) const
00085 {
00086   msg.header = header;  
00087 
00088   msg.route_points.resize(points.size());
00089   for (size_t i = 0; i < points.size(); ++i) {
00090     msgFromPoint(msg.route_points[i], points[i]);
00091   }
00092   
00093   std::vector<std::string> names = getPropertyNames();
00094   msg.properties.resize(names.size());
00095   for (size_t i = 0; i < names.size(); ++i) {
00096     msg.properties[i].key = names[i];
00097     msg.properties[i].value = getProperty(names[i]);
00098   }  
00099 }
00100 
00101 mnm::RoutePtr Route::toMsgPtr() const
00102 {
00103   mnm::RoutePtr ptr = boost::make_shared<mnm::Route>();
00104   toMsg(*ptr);
00105   return ptr;
00106 }
00107 
00108 bool Route::valid() const
00109 {
00110   return !points.empty();
00111 }
00112 
00113 bool Route::findPointId(size_t &index, const std::string &id) const
00114 {
00115   if (point_index_.count(id)) {
00116     size_t i = point_index_.at(id);
00117     if (i < points.size() && points[i].id() == id) {
00118       // This is a cache hit!
00119       index = i;
00120       return true;
00121     }
00122 
00123     // This is cache miss... our cache is out of date.
00124   }
00125 
00126   // If we reach here, either our cache is out of date or the point
00127   // doesn't exist in the cache.  We will rebuild the cache index to
00128   // make sure it is current.
00129   rebuildPointIndex();
00130 
00131   if (point_index_.count(id)) {
00132     // If the point was found, we expect the id to be valid (unless
00133     // you're using this class from multiple threads, which is not
00134     // supported.
00135     index = point_index_.at(id);
00136     return true;
00137   }
00138 
00139   // The point does not exist in this route.
00140   return false;
00141 }
00142 
00143 bool Route::findPointIdConst(size_t &index, const std::string &id) const
00144 {
00145   if (point_index_.count(id)) {
00146     size_t i = point_index_.at(id);
00147     if (i < points.size() && points[i].id() == id) {
00148       // This is a cache hit!
00149       index = i;
00150       return true;
00151     }
00152     // This is cache miss... our cache is out of date.    
00153   }
00154 
00155   return false;
00156 }
00157 
00158 std::vector<std::string> Route::getPropertyNames() const
00159 {
00160   std::vector<std::string> names;
00161   names.push_back("name");
00162   names.push_back("guid");
00163 
00164   for (auto const &it : properties_) {
00165     names.push_back(it.first);
00166   }
00167   
00168   return names;
00169 }
00170   
00171 std::string Route::getProperty(const std::string &name) const
00172 {
00173   if (name == "name") {
00174     return name_;
00175   } else if (name == "guid") {
00176     return guid_;
00177   } else if (properties_.count(name)) {
00178     return properties_.at(name);
00179   } else {
00180     return "";
00181   }
00182 }
00183 
00184 bool Route::hasProperty(const std::string &name) const
00185 {
00186   if (name == "name") {
00187     return true;
00188   } else if (name == "guid") {
00189     return true;
00190   } else {
00191     return properties_.count(name) > 0;
00192   }
00193 }
00194 
00195 void Route::setProperty(const std::string &name, const std::string &value)
00196 {
00197   if (name == "name") {
00198     name_ = value;
00199   } else if (name == "guid") {
00200     guid_ = value;
00201   } else {
00202     properties_[name] = value;
00203   } 
00204 }
00205 
00206 void Route::deleteProperty(const std::string &name)
00207 {
00208   // If we add "native" properties that are erasable, we should check
00209   // for those here first and mark them as deleted when appropriate.
00210   
00211   // Otherwise, fall back to the generic properties.
00212   // std::map::erase() ignores the call if the key is not found.
00213   properties_.erase(name);
00214 }
00215 
00216 std::string Route::name() const
00217 {
00218   return name_;
00219 }
00220 
00221 void Route::setName(const std::string &name)
00222 {
00223   name_ = name;
00224 }
00225   
00226 std::string Route::guid() const
00227 {
00228   return guid_;
00229 }
00230 
00231 void Route::setGuid(const std::string &guid)
00232 {
00233   guid_ = guid;
00234 }
00235 
00236 void Route::rebuildPointIndex() const
00237 {
00238   // Throw away the old index.
00239   point_index_.clear();
00240   for (size_t i = 0; i < points.size(); ++i) {
00241     point_index_[points[i].id()] = i;
00242   }
00243 
00244   if (point_index_.size() != points.size()) {
00245     ROS_ERROR("Route points do not have unique IDs.  This will likely cause problems.");
00246   }
00247 }
00248 }  // namespace swri_route_util


swri_route_util
Author(s):
autogenerated on Tue Oct 3 2017 03:19:53