route.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2016, Southwest Research Institute® (SwRI®)
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // * Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of Southwest Research Institute® (SwRI®) nor the
14 // names of its contributors may be used to endorse or promote products
15 // derived from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //
28 // *****************************************************************************
29 #include <swri_route_util/route.h>
31 #include <boost/make_shared.hpp>
32 
33 namespace mnm = marti_nav_msgs;
34 
35 namespace swri_route_util
36 {
38 {
39 }
40 
41 // Helper function used by the Route(mnm::Route) constructor.
42 static
43 void pointFromMsg(RoutePoint &dst, const marti_nav_msgs::RoutePoint &src)
44 {
45  dst.setPose(src.pose);
46  dst.setId(src.id);
47 
48  for (auto const &prop : src.properties) {
49  dst.setProperty(prop.key, prop.value);
50  }
51 }
52 
53 Route::Route(const mnm::Route &msg)
54 {
55  header = msg.header;
56 
57  points.resize(msg.route_points.size());
58  for (size_t i = 0; i < points.size(); ++i) {
59  pointFromMsg(points[i], msg.route_points[i]);
60  }
61 
62  for (auto const &prop : msg.properties) {
63  setProperty(prop.key, prop.value);
64  }
65 
67 }
68 
69 // Helper method used by the toMsg() method.
70 static
71 void msgFromPoint(marti_nav_msgs::RoutePoint &dst, const RoutePoint &src)
72 {
73  dst.pose = src.poseMsg();
74  dst.id = src.id();
75 
76  std::vector<std::string> names = src.getPropertyNames();
77  dst.properties.resize(names.size());
78  for (size_t i = 0; i < names.size(); ++i) {
79  dst.properties[i].key = names[i];
80  dst.properties[i].value = src.getProperty(names[i]);
81  }
82 }
83 
84 void Route::toMsg(mnm::Route &msg) const
85 {
86  msg.header = header;
87 
88  msg.route_points.resize(points.size());
89  for (size_t i = 0; i < points.size(); ++i) {
90  msgFromPoint(msg.route_points[i], points[i]);
91  }
92 
93  std::vector<std::string> names = getPropertyNames();
94  msg.properties.resize(names.size());
95  for (size_t i = 0; i < names.size(); ++i) {
96  msg.properties[i].key = names[i];
97  msg.properties[i].value = getProperty(names[i]);
98  }
99 }
100 
102 {
103  mnm::RoutePtr ptr = boost::make_shared<mnm::Route>();
104  toMsg(*ptr);
105  return ptr;
106 }
107 
108 bool Route::valid() const
109 {
110  return !points.empty();
111 }
112 
113 bool Route::findPointId(size_t &index, const std::string &id) const
114 {
115  if (point_index_.count(id)) {
116  size_t i = point_index_.at(id);
117  if (i < points.size() && points[i].id() == id) {
118  // This is a cache hit!
119  index = i;
120  return true;
121  }
122 
123  // This is cache miss... our cache is out of date.
124  }
125 
126  // If we reach here, either our cache is out of date or the point
127  // doesn't exist in the cache. We will rebuild the cache index to
128  // make sure it is current.
130 
131  if (point_index_.count(id)) {
132  // If the point was found, we expect the id to be valid (unless
133  // you're using this class from multiple threads, which is not
134  // supported.
135  index = point_index_.at(id);
136  return true;
137  }
138 
139  // The point does not exist in this route.
140  return false;
141 }
142 
143 bool Route::findPointIdConst(size_t &index, const std::string &id) const
144 {
145  if (point_index_.count(id)) {
146  size_t i = point_index_.at(id);
147  if (i < points.size() && points[i].id() == id) {
148  // This is a cache hit!
149  index = i;
150  return true;
151  }
152  // This is cache miss... our cache is out of date.
153  }
154 
155  return false;
156 }
157 
158 std::vector<std::string> Route::getPropertyNames() const
159 {
160  std::vector<std::string> names;
161  names.push_back("name");
162  names.push_back("guid");
163 
164  for (auto const &it : properties_) {
165  names.push_back(it.first);
166  }
167 
168  return names;
169 }
170 
171 std::string Route::getProperty(const std::string &name) const
172 {
173  if (name == "name") {
174  return name_;
175  } else if (name == "guid") {
176  return guid_;
177  } else if (properties_.count(name)) {
178  return properties_.at(name);
179  } else {
180  return "";
181  }
182 }
183 
184 bool Route::hasProperty(const std::string &name) const
185 {
186  if (name == "name") {
187  return true;
188  } else if (name == "guid") {
189  return true;
190  } else {
191  return properties_.count(name) > 0;
192  }
193 }
194 
195 void Route::setProperty(const std::string &name, const std::string &value)
196 {
197  if (name == "name") {
198  name_ = value;
199  } else if (name == "guid") {
200  guid_ = value;
201  } else {
202  properties_[name] = value;
203  }
204 }
205 
206 void Route::deleteProperty(const std::string &name)
207 {
208  // If we add "native" properties that are erasable, we should check
209  // for those here first and mark them as deleted when appropriate.
210 
211  // Otherwise, fall back to the generic properties.
212  // std::map::erase() ignores the call if the key is not found.
213  properties_.erase(name);
214 }
215 
216 std::string Route::name() const
217 {
218  return name_;
219 }
220 
221 void Route::setName(const std::string &name)
222 {
223  name_ = name;
224 }
225 
226 std::string Route::guid() const
227 {
228  return guid_;
229 }
230 
231 void Route::setGuid(const std::string &guid)
232 {
233  guid_ = guid;
234 }
235 
237 {
238  // Throw away the old index.
239  point_index_.clear();
240  for (size_t i = 0; i < points.size(); ++i) {
241  point_index_[points[i].id()] = i;
242  }
243 
244  if (point_index_.size() != points.size()) {
245  ROS_ERROR("Route points do not have unique IDs. This will likely cause problems.");
246  }
247 }
248 } // namespace swri_route_util
void setPose(const tf::Pose &pose)
msg
void rebuildPointIndex() const
Definition: route.cpp:236
std::string guid() const
Definition: route.cpp:226
void setGuid(const std::string &guid)
Definition: route.cpp:231
std::vector< std::string > getPropertyNames() const
Definition: route.cpp:158
void setProperty(const std::string &name, const std::string &value)
Definition: route.cpp:195
std_msgs::Header header
Definition: route.h:77
std::map< std::string, std::string > properties_
Definition: route.h:161
bool valid() const
Definition: route.cpp:108
static void msgFromPoint(marti_nav_msgs::RoutePoint &dst, const RoutePoint &src)
Definition: route.cpp:71
std::map< std::string, size_t > point_index_
Definition: route.h:158
marti_nav_msgs::RoutePtr toMsgPtr() const
Definition: route.cpp:101
void setName(const std::string &name)
Definition: route.cpp:221
std::string name_
Definition: route.h:165
static void pointFromMsg(RoutePoint &dst, const marti_nav_msgs::RoutePoint &src)
Definition: route.cpp:43
std::string name() const
Definition: route.cpp:216
void setId(const std::string &id)
std::string guid_
Definition: route.h:164
bool findPointId(size_t &index, const std::string &id) const
Definition: route.cpp:113
bool hasProperty(const std::string &name) const
Definition: route.cpp:184
void toMsg(marti_nav_msgs::Route &msg) const
Definition: route.cpp:84
std::vector< RoutePoint > points
Definition: route.h:82
const std::string & id() const
boost::shared_ptr< Route > RoutePtr
Definition: route.h:170
bool findPointIdConst(size_t &index, const std::string &id) const
Definition: route.cpp:143
std::string getProperty(const std::string &name) const
Definition: route.cpp:171
void setProperty(const std::string &name, const std::string &value)
Definition: route_point.cpp:76
#define ROS_ERROR(...)
std::string getProperty(const std::string &name) const
Definition: route_point.cpp:52
geometry_msgs::Pose poseMsg() const
void deleteProperty(const std::string &name)
Definition: route.cpp:206
std::vector< std::string > getPropertyNames() const
Definition: route_point.cpp:39


swri_route_util
Author(s):
autogenerated on Fri Jun 7 2019 22:06:09