Add costmap_2d package sources

Convert navigations/costmap_2d from gitlink to normal tracked files.

navigations/costmap_2d/src/array_parser.cpp

@@ -0,0 +1,115 @@
+/*
+ * Copyright (c) 2012, 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: Dave Hershberger
+ */
+
+#include <cstdio>  // for EOF
+#include <string>
+#include <sstream>
+#include <vector>
+
+namespace costmap_2d
+{
+
+/** @brief Parse a vector of vector of floats from a string.
+ * @param input
+ * @param error_return
+ * Syntax is [[1.0, 2.0], [3.3, 4.4, 5.5], ...] */
+std::vector<std::vector<float> > parseVVF(const std::string& input, std::string& error_return)
+{
+  std::vector<std::vector<float> > result;
+
+  std::stringstream input_ss(input);
+  int depth = 0;
+  std::vector<float> current_vector;
+  while (!!input_ss && !input_ss.eof())
+  {
+    switch (input_ss.peek())
+    {
+    case EOF:
+      break;
+    case '[':
+      depth++;
+      if (depth > 2)
+      {
+        error_return = "Array depth greater than 2";
+        return result;
+      }
+      input_ss.get();
+      current_vector.clear();
+      break;
+    case ']':
+      depth--;
+      if (depth < 0)
+      {
+        error_return = "More close ] than open [";
+        return result;
+      }
+      input_ss.get();
+      if (depth == 1)
+      {
+        result.push_back(current_vector);
+      }
+      break;
+    case ',':
+    case ' ':
+    case '\t':
+      input_ss.get();
+      break;
+    default:  // All other characters should be part of the numbers.
+      if (depth != 2)
+      {
+        std::stringstream err_ss;
+        err_ss << "Numbers at depth other than 2. Char was '" << char(input_ss.peek()) << "'.";
+        error_return = err_ss.str();
+        return result;
+      }
+      float value;
+      input_ss >> value;
+      if (!!input_ss)
+      {
+        current_vector.push_back(value);
+      }
+      break;
+    }
+  }
+
+  if (depth != 0)
+  {
+    error_return = "Unterminated vector string.";
+  }
+  else
+  {
+    error_return = "";
+  }
+
+  return result;
+}
+
+}  // end namespace costmap_2d