/* * 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