1726_24102025

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

src/costmap_2d.cpp

@@ -1,71 +1,445 @@
-#include <costmap_2d/costmap_2d.h>
-#include <cstdio>
+/*********************************************************************
+ * License & Authors
+ *  - Cung cấp lớp Costmap2D để lưu trữ bản đồ chi phí 2D
+ *********************************************************************/
+
+#include <costmap_2d/costmap_2d.h>  // Header định nghĩa lớp Costmap2D
+#include <cstdio>                   // Dùng cho thao tác file (fopen, fprintf, ...)
 
 using namespace std;
 
 namespace costmap_2d
 {
-    Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
-        double origin_x, double origin_y, unsigned char default_value) :
+
+/*****************************************************
+ *  Hàm khởi tạo chính của Costmap2D
+ *  - Tạo bản đồ kích thước size_x × size_y
+ *  - Gán độ phân giải, gốc tọa độ, và giá trị mặc định
+ *****************************************************/
+Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
+                     double origin_x, double origin_y, unsigned char default_value) :
     size_x_(cells_size_x), size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x),
     origin_y_(origin_y), costmap_(NULL), default_value_(default_value)
-    {
-        access_ = new mutex_t();
+{
+  access_ = new mutex_t();  // Khóa bảo vệ truy cập đa luồng
 
-        // create the costmap
-        initMaps(size_x_, size_y_);
-        resetMaps();
+  // Cấp phát vùng nhớ cho bản đồ
+  initMaps(size_x_, size_y_);
+
+  // Gán toàn bộ bản đồ bằng giá trị mặc định (ví dụ: NO_INFORMATION)
+  resetMaps();
+}
+
+/*****************************************************
+ *  Xóa bộ nhớ costmap_
+ *****************************************************/
+void Costmap2D::deleteMaps()
+{
+  boost::unique_lock<mutex_t> lock(*access_);  // Giữ mutex để đảm bảo thread-safe
+  delete[] costmap_;                           // Giải phóng vùng nhớ
+  costmap_ = NULL;
+}
+
+/*****************************************************
+ *  Cấp phát vùng nhớ cho costmap_
+ *****************************************************/
+void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y)
+{
+  boost::unique_lock<mutex_t> lock(*access_);
+  delete[] costmap_;                      // Xóa vùng nhớ cũ nếu có
+  costmap_ = new unsigned char[size_x * size_y];  // Mỗi ô lưu 1 byte (0–255)
+}
+
+/*****************************************************
+ *  Thay đổi kích thước bản đồ
+ *  - Thường dùng khi tạo lại bản đồ mới hoặc khi map động
+ *****************************************************/
+void Costmap2D::resizeMap(unsigned int size_x, unsigned int size_y, double resolution,
+                          double origin_x, double origin_y)
+{
+  size_x_ = size_x;
+  size_y_ = size_y;
+  resolution_ = resolution;
+  origin_x_ = origin_x;
+  origin_y_ = origin_y;
+
+  initMaps(size_x, size_y);  // Cấp phát vùng nhớ mới
+  resetMaps();               // Reset toàn bộ dữ liệu
+}
+
+/*****************************************************
+ *  Reset toàn bộ bản đồ về giá trị mặc định
+ *****************************************************/
+void Costmap2D::resetMaps()
+{
+  boost::unique_lock<mutex_t> lock(*access_);
+  memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char));
+}
+
+/*****************************************************
+ *  Reset một vùng con trong costmap
+ *****************************************************/
+void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
+{
+  boost::unique_lock<mutex_t> lock(*(access_));
+  unsigned int len = xn - x0;
+  for (unsigned int y = y0 * size_x_ + x0; y < yn * size_x_ + x0; y += size_x_)
+    memset(costmap_ + y, default_value_, len * sizeof(unsigned char));
+}
+
+/*****************************************************
+ *  Tạo bản đồ mới chỉ bao gồm 1 cửa sổ (window) con của map hiện tại
+ *****************************************************/
+bool Costmap2D::copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y,
+                                  double win_size_x, double win_size_y)
+{
+  // Không thể tạo window của chính mình
+  if (this == &map)
+    return false;
+
+  deleteMaps();  // Xóa dữ liệu cũ
+
+  // Chuyển đổi tọa độ thế giới sang tọa độ lưới
+  unsigned int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
+  if (!map.worldToMap(win_origin_x, win_origin_y, lower_left_x, lower_left_y)
+      || !map.worldToMap(win_origin_x + win_size_x, win_origin_y + win_size_y, upper_right_x, upper_right_y))
+    return false;
+
+  // Tính kích thước vùng con
+  size_x_ = upper_right_x - lower_left_x;
+  size_y_ = upper_right_y - lower_left_y;
+  resolution_ = map.resolution_;
+  origin_x_ = win_origin_x;
+  origin_y_ = win_origin_y;
+
+  initMaps(size_x_, size_y_);  // Cấp phát bộ nhớ mới
+
+  // Sao chép dữ liệu vùng con từ bản đồ gốc
+  copyMapRegion(map.costmap_, lower_left_x, lower_left_y, map.size_x_,
+                costmap_, 0, 0, size_x_, size_x_, size_y_);
+  return true;
+}
+
+/*****************************************************
+ *  Toán tử gán (=): sao chép toàn bộ dữ liệu từ map khác
+ *****************************************************/
+Costmap2D& Costmap2D::operator=(const Costmap2D& map)
+{
+  if (this == &map)
+    return *this;
+
+  deleteMaps();  // Xóa map cũ
+
+  size_x_ = map.size_x_;
+  size_y_ = map.size_y_;
+  resolution_ = map.resolution_;
+  origin_x_ = map.origin_x_;
+  origin_y_ = map.origin_y_;
+
+  initMaps(size_x_, size_y_);  // Tạo map mới
+  memcpy(costmap_, map.costmap_, size_x_ * size_y_ * sizeof(unsigned char)); // Sao chép dữ liệu
+
+  return *this;
+}
+
+/*****************************************************
+ *  Constructor sao chép
+ *****************************************************/
+Costmap2D::Costmap2D(const Costmap2D& map) : costmap_(NULL)
+{
+  access_ = new mutex_t();
+  *this = map;  // Gọi lại toán tử gán
+}
+
+/*****************************************************
+ *  Constructor mặc định (chưa có bản đồ)
+ *****************************************************/
+Costmap2D::Costmap2D() :
+    size_x_(0), size_y_(0), resolution_(0.0),
+    origin_x_(0.0), origin_y_(0.0), costmap_(NULL)
+{
+  access_ = new mutex_t();
+}
+
+/*****************************************************
+ *  Destructor: giải phóng bộ nhớ
+ *****************************************************/
+Costmap2D::~Costmap2D()
+{
+  deleteMaps();
+  delete access_;
+}
+
+/*****************************************************
+ *  Chuyển khoảng cách thực (mét) sang đơn vị ô
+ *****************************************************/
+unsigned int Costmap2D::cellDistance(double world_dist)
+{
+  double cells_dist = max(0.0, ceil(world_dist / resolution_));
+  return (unsigned int)cells_dist;
+}
+
+/*****************************************************
+ *  Trả về con trỏ dữ liệu costmap
+ *****************************************************/
+unsigned char* Costmap2D::getCharMap() const
+{
+  return costmap_;
+}
+
+/*****************************************************
+ *  Lấy giá trị cost tại ô (mx, my)
+ *****************************************************/
+unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const
+{
+  return costmap_[getIndex(mx, my)];
+}
+
+/*****************************************************
+ *  Gán giá trị cost tại ô (mx, my)
+ *****************************************************/
+void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost)
+{
+  costmap_[getIndex(mx, my)] = cost;
+}
+
+/*****************************************************
+ *  Chuyển từ tọa độ lưới (ô) sang tọa độ thế giới
+ *****************************************************/
+void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const
+{
+  wx = origin_x_ + (mx + 0.5) * resolution_;
+  wy = origin_y_ + (my + 0.5) * resolution_;
+}
+
+/*****************************************************
+ *  Chuyển từ tọa độ thế giới sang tọa độ lưới
+ *****************************************************/
+bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const
+{
+  if (wx < origin_x_ || wy < origin_y_)
+    return false;
+
+  mx = (int)((wx - origin_x_) / resolution_);
+  my = (int)((wy - origin_y_) / resolution_);
+
+  return (mx < size_x_ && my < size_y_);
+}
+
+/*****************************************************
+ *  Phiên bản không kiểm tra biên (có thể ra ngoài map)
+ *****************************************************/
+void Costmap2D::worldToMapNoBounds(double wx, double wy, int& mx, int& my) const
+{
+  mx = (int)((wx - origin_x_) / resolution_);
+  my = (int)((wy - origin_y_) / resolution_);
+}
+
+/*****************************************************
+ *  Phiên bản ép buộc tọa độ nằm trong biên bản đồ
+ *****************************************************/
+void Costmap2D::worldToMapEnforceBounds(double wx, double wy, int& mx, int& my) const
+{
+  if (wx < origin_x_)
+    mx = 0;
+  else if (wx >= resolution_ * size_x_ + origin_x_)
+    mx = size_x_ - 1;
+  else
+    mx = (int)((wx - origin_x_) / resolution_);
+
+  if (wy < origin_y_)
+    my = 0;
+  else if (wy >= resolution_ * size_y_ + origin_y_)
+    my = size_y_ - 1;
+  else
+    my = (int)((wy - origin_y_) / resolution_);
+}
+
+/*****************************************************
+ *  Dịch chuyển gốc của bản đồ khi robot di chuyển
+ *  => Dữ liệu được dịch và giữ lại phần chồng lấn
+ *****************************************************/
+void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y)
+{
+  int cell_ox = int((new_origin_x - origin_x_) / resolution_);
+  int cell_oy = int((new_origin_y - origin_y_) / resolution_);
+
+  if (cell_ox == 0 && cell_oy == 0)
+    return;
+
+  // Cập nhật lại gốc bản đồ mới (theo bội số của resolution)
+  double new_grid_ox = origin_x_ + cell_ox * resolution_;
+  double new_grid_oy = origin_y_ + cell_oy * resolution_;
+
+  int size_x = size_x_;
+  int size_y = size_y_;
+
+  // Tính vùng chồng lấn giữa bản đồ cũ và bản đồ mới
+  int lower_left_x = min(max(cell_ox, 0), size_x);
+  int lower_left_y = min(max(cell_oy, 0), size_y);
+  int upper_right_x = min(max(cell_ox + size_x, 0), size_x);
+  int upper_right_y = min(max(cell_oy + size_y, 0), size_y);
+
+  unsigned int cell_size_x = upper_right_x - lower_left_x;
+  unsigned int cell_size_y = upper_right_y - lower_left_y;
+
+  // Tạo bản đồ tạm để lưu phần chồng lấn
+  unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
+  copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_,
+                local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
+
+  resetMaps(); // Toàn bộ bản đồ được đặt lại giá trị mặc định
+
+  origin_x_ = new_grid_ox;
+  origin_y_ = new_grid_oy;
+
+  // Copy vùng chồng lấn vào vị trí mới
+  int start_x = lower_left_x - cell_ox;
+  int start_y = lower_left_y - cell_oy;
+  copyMapRegion(local_map, 0, 0, cell_size_x,
+                costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
+
+  delete[] local_map; // Giải phóng vùng nhớ tạm
+}
+
+/*****************************************************
+ *  Đặt giá trị cost cho vùng đa giác lồi
+ *  (ví dụ: để đánh dấu vùng cấm hoặc vùng obstacle)
+ *****************************************************/
+bool Costmap2D::setConvexPolygonCost(const std::vector<Point>& polygon, unsigned char cost_value)
+{
+  std::vector<MapLocation> map_polygon;
+  for (unsigned int i = 0; i < polygon.size(); ++i)
+  {
+    MapLocation loc;
+    if (!worldToMap(polygon[i].x, polygon[i].y, loc.x, loc.y))
+      return false; // Nếu một điểm nằm ngoài map
+    map_polygon.push_back(loc);
+  }
+
+  std::vector<MapLocation> polygon_cells;
+  convexFillCells(map_polygon, polygon_cells); // Lấy toàn bộ ô bên trong đa giác
+
+  for (unsigned int i = 0; i < polygon_cells.size(); ++i)
+    costmap_[getIndex(polygon_cells[i].x, polygon_cells[i].y)] = cost_value;
+
+  return true;
+}
+
+/*****************************************************
+ *  polygonOutlineCells():
+ *  - Tìm các ô nằm trên biên của đa giác
+ *****************************************************/
+void Costmap2D::polygonOutlineCells(const std::vector<MapLocation>& polygon,
+                                    std::vector<MapLocation>& polygon_cells)
+{
+  PolygonOutlineCells cell_gatherer(*this, costmap_, polygon_cells);
+
+  for (unsigned int i = 0; i < polygon.size() - 1; ++i)
+    raytraceLine(cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y);
+
+  if (!polygon.empty())
+  {
+    unsigned int last_index = polygon.size() - 1;
+    // Nối điểm cuối với điểm đầu để khép kín đa giác
+    raytraceLine(cell_gatherer, polygon[last_index].x, polygon[last_index].y,
+                 polygon[0].x, polygon[0].y);
+  }
+}
+
+/*****************************************************
+ *  convexFillCells():
+ *  - Điền đầy các ô bên trong một đa giác lồi
+ *****************************************************/
+void Costmap2D::convexFillCells(const std::vector<MapLocation>& polygon,
+                                std::vector<MapLocation>& polygon_cells)
+{
+  if (polygon.size() < 3)  // Ít hơn 3 điểm thì không tạo thành đa giác
+    return;
+
+  polygonOutlineCells(polygon, polygon_cells);  // Lấy biên ngoài
+
+  // Sắp xếp theo trục X để dễ quét cột
+  MapLocation swap;
+  unsigned int i = 0;
+  while (i < polygon_cells.size() - 1)
+  {
+    if (polygon_cells[i].x > polygon_cells[i + 1].x)
+    {
+      swap = polygon_cells[i];
+      polygon_cells[i] = polygon_cells[i + 1];
+      polygon_cells[i + 1] = swap;
+      if (i > 0) --i;
+    }
+    else ++i;
+  }
+
+  // Quét từng cột X để điền đầy Y
+  i = 0;
+  MapLocation min_pt, max_pt;
+  unsigned int min_x = polygon_cells[0].x;
+  unsigned int max_x = polygon_cells.back().x;
+
+  for (unsigned int x = min_x; x <= max_x; ++x)
+  {
+    if (i >= polygon_cells.size() - 1) break;
+
+    if (polygon_cells[i].y < polygon_cells[i + 1].y)
+    {
+      min_pt = polygon_cells[i];
+      max_pt = polygon_cells[i + 1];
+    }
+    else
+    {
+      min_pt = polygon_cells[i + 1];
+      max_pt = polygon_cells[i];
     }
 
-    void Costmap2D::deleteMaps()
+    i += 2;
+    while (i < polygon_cells.size() && polygon_cells[i].x == x)
     {
-        // clean up data
-        boost::unique_lock<mutex_t> lock(*access_);
-        delete[] costmap_;
-        costmap_ = NULL;
+      if (polygon_cells[i].y < min_pt.y)
+        min_pt = polygon_cells[i];
+      else if (polygon_cells[i].y > max_pt.y)
+        max_pt = polygon_cells[i];
+      ++i;
     }
 
-    void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y)
-    {
-        boost::unique_lock<mutex_t> lock(*access_);
-        delete[] costmap_;
-        costmap_ = new unsigned char[size_x * size_y];
-    }
+    // Điền đầy ô từ min_y đến max_y
+    for (unsigned int y = min_pt.y; y <= max_pt.y; ++y)
+      polygon_cells.push_back({x, y});
+  }
+}
 
-    unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const
-    {
-        return costmap_[getIndex(mx, my)];
-    }
+/*****************************************************
+ *  Các hàm getter cơ bản
+ *****************************************************/
+unsigned int Costmap2D::getSizeInCellsX() const { return size_x_; }
+unsigned int Costmap2D::getSizeInCellsY() const { return size_y_; }
+double Costmap2D::getSizeInMetersX() const { return (size_x_ - 1 + 0.5) * resolution_; }
+double Costmap2D::getSizeInMetersY() const { return (size_y_ - 1 + 0.5) * resolution_; }
+double Costmap2D::getOriginX() const { return origin_x_; }
+double Costmap2D::getOriginY() const { return origin_y_; }
+double Costmap2D::getResolution() const { return resolution_; }
 
-    void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost)
-    {
-        costmap_[getIndex(mx, my)] = cost;
-    }
+/*****************************************************
+ *  Lưu costmap ra file PGM (grayscale) để debug
+ *****************************************************/
+bool Costmap2D::saveMap(std::string file_name)
+{
+  FILE *fp = fopen(file_name.c_str(), "w");
+  if (!fp)
+    return false;
 
-    void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const
-    {
-        wx = origin_x_ + (mx + 0.5) * resolution_;
-        wy = origin_y_ + (my + 0.5) * resolution_;
-    }
+  fprintf(fp, "P2\n%u\n%u\n%u\n", size_x_, size_y_, 0xff);
+  for (unsigned int iy = 0; iy < size_y_; iy++)
+  {
+    for (unsigned int ix = 0; ix < size_x_; ix++)
+      fprintf(fp, "%d ", getCost(ix, iy));
+    fprintf(fp, "\n");
+  }
+  fclose(fp);
+  return true;
+}
 
-    bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const
-    {
-        if (wx < origin_x_ || wy < origin_y_)
-            return false;
-
-        mx = (int)((wx - origin_x_) / resolution_);
-        my = (int)((wy - origin_y_) / resolution_);
-
-        if (mx < size_x_ && my < size_y_)
-            return true;
-
-        return false;
-    }
-
-    void Costmap2D::worldToMapNoBounds(double wx, double wy, int& mx, int& my) const
-    {
-        mx = (int)((wx - origin_x_) / resolution_);
-        my = (int)((wy - origin_y_) / resolution_);
-    }
-
-}
+}  // namespace costmap_2d

src/footprint.cpp

@@ -0,0 +1,325 @@
+/*
+ * Copyright (c) 2013, 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.
+ */
+
+#include<costmap_2d/costmap_math.h>
+#include <boost/tokenizer.hpp>
+#include <boost/foreach.hpp>
+#include <boost/algorithm/string.hpp>
+#include <costmap_2d/footprint.h>
+#include <costmap_2d/array_parser.h>
+#include<costmap_2d/msg.h>
+
+namespace costmap_2d
+{
+
+void calculateMinAndMaxDistances(const std::vector<Point>& footprint, double& min_dist, double& max_dist)
+{
+  min_dist = std::numeric_limits<double>::max();
+  max_dist = 0.0;
+
+  if (footprint.size() <= 2)
+  {
+    return;
+  }
+
+  for (unsigned int i = 0; i < footprint.size() - 1; ++i)
+  {
+    // check the distance from the robot center point to the first vertex
+    double vertex_dist = distance(0.0, 0.0, footprint[i].x, footprint[i].y);
+    double edge_dist = distanceToLine(0.0, 0.0, footprint[i].x, footprint[i].y,
+                                      footprint[i + 1].x, footprint[i + 1].y);
+    min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
+    max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
+  }
+
+  // we also need to do the last vertex and the first vertex
+  double vertex_dist = distance(0.0, 0.0, footprint.back().x, footprint.back().y);
+  double edge_dist = distanceToLine(0.0, 0.0, footprint.back().x, footprint.back().y,
+                                      footprint.front().x, footprint.front().y);
+  min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
+  max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
+}
+
+Point32 toPoint32(Point pt)
+{
+  Point32 point32;
+  point32.x = pt.x;
+  point32.y = pt.y;
+  point32.z = pt.z;
+  return point32;
+}
+
+Point toPoint(Point32 pt)
+{
+  Point point;
+  point.x = pt.x;
+  point.y = pt.y;
+  point.z = pt.z;
+  return point;
+}
+
+Polygon toPolygon(std::vector<Point> pts)
+{
+  Polygon polygon;
+  for (int i = 0; i < pts.size(); i++){
+    polygon.points.push_back(toPoint32(pts[i]));
+  }
+  return polygon;
+}
+
+std::vector<Point> toPointVector(Polygon polygon)
+{
+  std::vector<Point> pts;
+  for (int i = 0; i < polygon.points.size(); i++)
+  {
+    pts.push_back(toPoint(polygon.points[i]));
+  }
+  return pts;
+}
+
+void transformFootprint(double x, double y, double theta, const std::vector<Point>& footprint_spec,
+                        std::vector<Point>& oriented_footprint)
+{
+  // build the oriented footprint at a given location
+  oriented_footprint.clear();
+  double cos_th = cos(theta);
+  double sin_th = sin(theta);
+  for (unsigned int i = 0; i < footprint_spec.size(); ++i)
+  {
+    Point new_pt;
+    new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
+    new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
+    oriented_footprint.push_back(new_pt);
+  }
+}
+
+void transformFootprint(double x, double y, double theta, const std::vector<Point>& footprint_spec,
+                        PolygonStamped& oriented_footprint)
+{
+  // build the oriented footprint at a given location
+  oriented_footprint.polygon.points.clear();
+  double cos_th = cos(theta);
+  double sin_th = sin(theta);
+  for (unsigned int i = 0; i < footprint_spec.size(); ++i)
+  {
+    Point32 new_pt;
+    new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
+    new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
+    oriented_footprint.polygon.points.push_back(new_pt);
+  }
+}
+
+void padFootprint(std::vector<Point>& footprint, double padding)
+{
+  // pad footprint in place
+  for (unsigned int i = 0; i < footprint.size(); i++)
+  {
+    Point& pt = footprint[ i ];
+    pt.x += sign0(pt.x) * padding;
+    pt.y += sign0(pt.y) * padding;
+  }
+}
+
+
+std::vector<Point> makeFootprintFromRadius(double radius)
+{
+  std::vector<Point> points;
+
+  // Loop over 16 angles around a circle making a point each time
+  int N = 16;
+  Point pt;
+  for (int i = 0; i < N; ++i)
+  {
+    double angle = i * 2 * M_PI / N;
+    pt.x = cos(angle) * radius;
+    pt.y = sin(angle) * radius;
+
+    points.push_back(pt);
+  }
+
+  return points;
+}
+
+
+bool makeFootprintFromString(const std::string& footprint_string, std::vector<Point>& footprint)
+{
+  std::string error;
+  std::vector<std::vector<float> > vvf = parseVVF(footprint_string, error);
+
+  if (error != "")
+  {
+    std::printf("Error parsing footprint parameter: '%s'", error.c_str());
+    std::printf("  Footprint string was '%s'.", footprint_string.c_str());
+    return false;
+  }
+
+  // convert vvf into points.
+  if (vvf.size() < 3)
+  {
+    std::printf("You must specify at least three points for the robot footprint, reverting to previous footprint.");
+    return false;
+  }
+  footprint.reserve(vvf.size());
+  for (unsigned int i = 0; i < vvf.size(); i++)
+  {
+    if (vvf[ i ].size() == 2)
+    {
+      Point point;
+      point.x = vvf[ i ][ 0 ];
+      point.y = vvf[ i ][ 1 ];
+      point.z = 0;
+      footprint.push_back(point);
+    }
+    else
+    {
+      std::printf("Points in the footprint specification must be pairs of numbers.  Found a point with %d numbers.",
+                 int(vvf[ i ].size()));
+      return false;
+    }
+  }
+
+  return true;
+}
+
+
+
+// std::vector<Point> makeFootprintFromParams(ros::NodeHandle& nh)
+// {
+//   std::string full_param_name;
+//   std::string full_radius_param_name;
+//   std::vector<Point> points;
+
+//   if (nh.searchParam("footprint", full_param_name))
+//   {
+//     XmlRpc::XmlRpcValue footprint_xmlrpc;
+//     nh.getParam(full_param_name, footprint_xmlrpc);
+//     if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString &&
+//         footprint_xmlrpc != "" && footprint_xmlrpc != "[]")
+//     {
+//       if (makeFootprintFromString(std::string(footprint_xmlrpc), points))
+//       {
+//         writeFootprintToParam(nh, points);
+//         return points;
+//       }
+//     }
+//     else if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
+//     {
+//       points = makeFootprintFromXMLRPC(footprint_xmlrpc, full_param_name);
+//       writeFootprintToParam(nh, points);
+//       return points;
+//     }
+//   }
+
+//   if (nh.searchParam("robot_radius", full_radius_param_name))
+//   {
+//     double robot_radius;
+//     nh.param(full_radius_param_name, robot_radius, 1.234);
+//     points = makeFootprintFromRadius(robot_radius);
+//     nh.setParam("robot_radius", robot_radius);
+//   }
+//   // Else neither param was found anywhere this knows about, so
+//   // defaults will come from dynamic_reconfigure stuff, set in
+//   // cfg/Costmap2D.cfg and read in this file in reconfigureCB().
+//   return points;
+// }
+
+// void writeFootprintToParam(std::string& nh, const std::vector<Point>& footprint)
+// {
+//   std::ostringstream oss;
+//   bool first = true;
+//   for (unsigned int i = 0; i < footprint.size(); i++)
+//   {
+//     Point p = footprint[ i ];
+//     if (first)
+//     {
+//       oss << "[[" << p.x << "," << p.y << "]";
+//       first = false;
+//     }
+//     else
+//     {
+//       oss << ",[" << p.x << "," << p.y << "]";
+//     }
+//   }
+//   oss << "]";
+//   nh.setParam("footprint", oss.str().c_str());
+// }
+
+// double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name)
+// {
+//   // Make sure that the value we're looking at is either a double or an int.
+//   if (value.getType() != XmlRpc::XmlRpcValue::TypeInt &&
+//       value.getType() != XmlRpc::XmlRpcValue::TypeDouble)
+//   {
+//     std::string& value_string = value;
+//     std::printf("Values in the footprint specification (param %s) must be numbers. Found value %s.",
+//                full_param_name.c_str(), value_string.c_str());
+//     throw std::runtime_error("Values in the footprint specification must be numbers");
+//   }
+//   return value.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value);
+// }
+
+// std::vector<Point> makeFootprintFromXMLRPC(XmlRpcValue& footprint_xmlrpc,
+//                                 const std::string& full_param_name)
+// {
+//   // Make sure we have an array of at least 3 elements.
+//   if (footprint_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray ||
+//       footprint_xmlrpc.size() < 3)
+//   {
+//     std::printf("The footprint must be specified as list of lists on the parameter server, %s was specified as %s",
+//                full_param_name.c_str(), std::string(footprint_xmlrpc).c_str());
+//     throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least "
+//                              "3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
+//   }
+
+//   std::vector<Point> footprint;
+//   Point pt;
+
+//   for (int i = 0; i < footprint_xmlrpc.size(); ++i)
+//   {
+//     // Make sure each element of the list is an array of size 2. (x and y coordinates)
+//     XmlRpc::XmlRpcValue point = footprint_xmlrpc[ i ];
+//     if (point.getType() != XmlRpc::XmlRpcValue::TypeArray ||
+//         point.size() != 2)
+//     {
+//       std::printf("The footprint (parameter %s) must be specified as list of lists on the parameter server eg: "
+//                 "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form.",
+//                  full_param_name.c_str());
+//       throw std::runtime_error("The footprint must be specified as list of lists on the parameter server eg: "
+//                                "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form");
+//     }
+
+//     pt.x = getNumberFromXMLRPC(point[ 0 ], full_param_name);
+//     pt.y = getNumberFromXMLRPC(point[ 1 ], full_param_name);
+
+//     footprint.push_back(pt);
+//   }
+//   return footprint;
+// }
+
+}  // end namespace costmap_2d

src/layer.cpp

@@ -0,0 +1,27 @@
+#include "costmap_2d/layer.h"
+
+namespace costmap_2d
+{
+
+Layer::Layer()
+  : layered_costmap_(NULL)
+  , current_(false)
+  , enabled_(false)
+  , name_()
+  , tf_(NULL)
+{}
+
+void Layer::initialize(LayeredCostmap* parent, std::string name, std::shared_ptr<tf2::BufferCore> *tf)
+{
+  layered_costmap_ = parent;
+  name_ = name;
+  tf_ = tf;
+  onInitialize();
+}
+
+const std::vector<Point>& Layer::getFootprint() const
+{
+  return layered_costmap_->getFootprint();
+}
+
+}  // end namespace costmap_2d

src/layered_costmap.cpp

@@ -0,0 +1,153 @@
+#include <costmap_2d/layered_costmap.h>
+#include <costmap_2d/footprint.h>
+#include <cstdio>
+#include <string>
+#include <algorithm>
+#include <vector>
+
+using std::vector;
+
+namespace costmap_2d
+{
+
+  LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown)
+      : costmap_(),
+        global_frame_(global_frame),
+        rolling_window_(rolling_window),
+        current_(false),
+        minx_(0.0),
+        miny_(0.0),
+        maxx_(0.0),
+        maxy_(0.0),
+        bx0_(0),
+        bxn_(0),
+        by0_(0),
+        byn_(0),
+        initialized_(false),
+        size_locked_(false),
+        circumscribed_radius_(1.0),
+        inscribed_radius_(0.1)
+  {
+    if (track_unknown)
+      costmap_.setDefaultValue(NO_INFORMATION);
+    else
+      costmap_.setDefaultValue(FREE_SPACE);
+  }
+
+  LayeredCostmap::~LayeredCostmap()
+  {
+    while (plugins_.size() > 0)
+    {
+      plugins_.pop_back();
+    }
+  }
+
+  void LayeredCostmap::resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x,
+                                 double origin_y, bool size_locked)
+  {
+    boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
+    size_locked_ = size_locked;
+    costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y);
+    for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
+         ++plugin)
+    {
+      (*plugin)->matchSize();
+    }
+  }
+
+  void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
+  {
+    // Lock for the remainder of this function, some plugins (e.g. VoxelLayer)
+    // implement thread unsafe updateBounds() functions.
+    boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
+
+    // if we're using a rolling buffer costmap... we need to update the origin using the robot's position
+    if (rolling_window_)
+    {
+      double new_origin_x = robot_x - costmap_.getSizeInMetersX() / 2;
+      double new_origin_y = robot_y - costmap_.getSizeInMetersY() / 2;
+      costmap_.updateOrigin(new_origin_x, new_origin_y);
+    }
+
+    if (plugins_.size() == 0)
+      return;
+
+    minx_ = miny_ = 1e30;
+    maxx_ = maxy_ = -1e30;
+
+    for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
+         ++plugin)
+    {
+      if (!(*plugin)->isEnabled())
+        continue;
+      double prev_minx = minx_;
+      double prev_miny = miny_;
+      double prev_maxx = maxx_;
+      double prev_maxy = maxy_;
+      (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
+      if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy)
+      {
+        std::printf("Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
+                               "is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s",
+                          prev_minx, prev_miny, prev_maxx, prev_maxy,
+                          minx_, miny_, maxx_, maxy_,
+                          (*plugin)->getName().c_str());
+      }
+    }
+
+    int x0, xn, y0, yn;
+    costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0);
+    costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn);
+
+    x0 = std::max(0, x0);
+    xn = std::min(int(costmap_.getSizeInCellsX()), xn + 1);
+    y0 = std::max(0, y0);
+    yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1);
+
+    std::printf("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn);
+    
+    if (xn < x0 || yn < y0)
+      return;
+
+    costmap_.resetMap(x0, y0, xn, yn);
+    
+    for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
+         ++plugin)
+    {
+      if ((*plugin)->isEnabled())
+        (*plugin)->updateCosts(costmap_, x0, y0, xn, yn);
+    }
+
+    bx0_ = x0;
+    bxn_ = xn;
+    by0_ = y0;
+    byn_ = yn;
+
+    initialized_ = true;
+  }
+
+  bool LayeredCostmap::isCurrent()
+  {
+    current_ = true;
+    for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
+         ++plugin)
+    {
+      if ((*plugin)->isEnabled())
+        current_ = current_ && (*plugin)->isCurrent();
+    }
+    return current_;
+  }
+
+  void LayeredCostmap::setFootprint(const std::vector<Point> &footprint_spec)
+  {
+    footprint_ = footprint_spec;
+    costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_);
+
+    for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
+         ++plugin)
+    {
+      (*plugin)->onFootprintChanged();
+    }
+  }
+
+} // namespace costmap_2d

src/observation_buffer.cpp

@@ -0,0 +1,251 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, 2013, 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 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: Eitan Marder-Eppstein
+ *********************************************************************/
+#include <costmap_2d/observation_buffer.h>
+
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
+#include <sensor_msgs/point_cloud2_iterator.h>
+
+using namespace std;
+using namespace tf2;
+
+namespace costmap_2d
+{
+ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate,
+                                     double min_obstacle_height, double max_obstacle_height, double obstacle_range,
+                                     double raytrace_range, tf2_ros::Buffer& tf2_buffer, string global_frame,
+                                     string sensor_frame, double tf_tolerance) :
+    tf2_buffer_(tf2_buffer), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
+    last_updated_(ros::Time::now()), global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
+    min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height),
+    obstacle_range_(obstacle_range), raytrace_range_(raytrace_range), tf_tolerance_(tf_tolerance)
+{
+}
+
+ObservationBuffer::~ObservationBuffer()
+{
+}
+
+bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
+{
+  ros::Time transform_time = ros::Time::now();
+  std::string tf_error;
+
+  geometry_msgs::TransformStamped transformStamped;
+  if (!tf2_buffer_.canTransform(new_global_frame, global_frame_, transform_time, ros::Duration(tf_tolerance_), &tf_error))
+  {
+    ROS_ERROR("Transform between %s and %s with tolerance %.2f failed: %s.", new_global_frame.c_str(),
+              global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
+    return false;
+  }
+
+  list<Observation>::iterator obs_it;
+  for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
+  {
+    try
+    {
+      Observation& obs = *obs_it;
+
+      geometry_msgs::PointStamped origin;
+      origin.header.frame_id = global_frame_;
+      origin.header.stamp = transform_time;
+      origin.point = obs.origin_;
+
+      // we need to transform the origin of the observation to the new global frame
+      tf2_buffer_.transform(origin, origin, new_global_frame);
+      obs.origin_ = origin.point;
+
+      // we also need to transform the cloud of the observation to the new global frame
+      tf2_buffer_.transform(*(obs.cloud_), *(obs.cloud_), new_global_frame);
+    }
+    catch (TransformException& ex)
+    {
+      ROS_ERROR("TF Error attempting to transform an observation from %s to %s: %s", global_frame_.c_str(),
+                new_global_frame.c_str(), ex.what());
+      return false;
+    }
+  }
+
+  // now we need to update our global_frame member
+  global_frame_ = new_global_frame;
+  return true;
+}
+
+void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
+{
+  geometry_msgs::PointStamped global_origin;
+
+  // create a new observation on the list to be populated
+  observation_list_.push_front(Observation());
+
+  // check whether the origin frame has been set explicitly or whether we should get it from the cloud
+  string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_;
+
+  try
+  {
+    // given these observations come from sensors... we'll need to store the origin pt of the sensor
+    geometry_msgs::PointStamped local_origin;
+    local_origin.header.stamp = cloud.header.stamp;
+    local_origin.header.frame_id = origin_frame;
+    local_origin.point.x = 0;
+    local_origin.point.y = 0;
+    local_origin.point.z = 0;
+    tf2_buffer_.transform(local_origin, global_origin, global_frame_);
+    tf2::convert(global_origin.point, observation_list_.front().origin_);
+
+    // make sure to pass on the raytrace/obstacle range of the observation buffer to the observations
+    observation_list_.front().raytrace_range_ = raytrace_range_;
+    observation_list_.front().obstacle_range_ = obstacle_range_;
+
+    sensor_msgs::PointCloud2 global_frame_cloud;
+
+    // transform the point cloud
+    tf2_buffer_.transform(cloud, global_frame_cloud, global_frame_);
+    global_frame_cloud.header.stamp = cloud.header.stamp;
+
+    // now we need to remove observations from the cloud that are below or above our height thresholds
+    sensor_msgs::PointCloud2& observation_cloud = *(observation_list_.front().cloud_);
+    observation_cloud.height = global_frame_cloud.height;
+    observation_cloud.width = global_frame_cloud.width;
+    observation_cloud.fields = global_frame_cloud.fields;
+    observation_cloud.is_bigendian = global_frame_cloud.is_bigendian;
+    observation_cloud.point_step = global_frame_cloud.point_step;
+    observation_cloud.row_step = global_frame_cloud.row_step;
+    observation_cloud.is_dense = global_frame_cloud.is_dense;
+
+    unsigned int cloud_size = global_frame_cloud.height*global_frame_cloud.width;
+    sensor_msgs::PointCloud2Modifier modifier(observation_cloud);
+    modifier.resize(cloud_size);
+    unsigned int point_count = 0;
+
+    // copy over the points that are within our height bounds
+    sensor_msgs::PointCloud2Iterator<float> iter_z(global_frame_cloud, "z");
+    std::vector<unsigned char>::const_iterator iter_global = global_frame_cloud.data.begin(), iter_global_end = global_frame_cloud.data.end();
+    std::vector<unsigned char>::iterator iter_obs = observation_cloud.data.begin();
+    for (; iter_global != iter_global_end; ++iter_z, iter_global += global_frame_cloud.point_step)
+    {
+      if ((*iter_z) <= max_obstacle_height_
+          && (*iter_z) >= min_obstacle_height_)
+      {
+        std::copy(iter_global, iter_global + global_frame_cloud.point_step, iter_obs);
+        iter_obs += global_frame_cloud.point_step;
+        ++point_count;
+      }
+    }
+
+    // resize the cloud for the number of legal points
+    modifier.resize(point_count);
+    observation_cloud.header.stamp = cloud.header.stamp;
+    observation_cloud.header.frame_id = global_frame_cloud.header.frame_id;
+  }
+  catch (TransformException& ex)
+  {
+    // if an exception occurs, we need to remove the empty observation from the list
+    observation_list_.pop_front();
+    ROS_ERROR("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", sensor_frame_.c_str(),
+              cloud.header.frame_id.c_str(), ex.what());
+    return;
+  }
+
+  // if the update was successful, we want to update the last updated time
+  last_updated_ = ros::Time::now();
+
+  // we'll also remove any stale observations from the list
+  purgeStaleObservations();
+}
+
+// returns a copy of the observations
+void ObservationBuffer::getObservations(vector<Observation>& observations)
+{
+  // first... let's make sure that we don't have any stale observations
+  purgeStaleObservations();
+
+  // now we'll just copy the observations for the caller
+  list<Observation>::iterator obs_it;
+  for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
+  {
+    observations.push_back(*obs_it);
+  }
+}
+
+void ObservationBuffer::purgeStaleObservations()
+{
+  if (!observation_list_.empty())
+  {
+    list<Observation>::iterator obs_it = observation_list_.begin();
+    // if we're keeping observations for no time... then we'll only keep one observation
+    if (observation_keep_time_ == ros::Duration(0.0))
+    {
+      observation_list_.erase(++obs_it, observation_list_.end());
+      return;
+    }
+
+    // otherwise... we'll have to loop through the observations to see which ones are stale
+    for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
+    {
+      Observation& obs = *obs_it;
+      // check if the observation is out of date... and if it is, remove it and those that follow from the list
+      if ((last_updated_ - obs.cloud_->header.stamp) > observation_keep_time_)
+      {
+        observation_list_.erase(obs_it, observation_list_.end());
+        return;
+      }
+    }
+  }
+}
+
+bool ObservationBuffer::isCurrent() const
+{
+  if (expected_update_rate_ == ros::Duration(0.0))
+    return true;
+
+  bool current = (ros::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();
+  if (!current)
+  {
+    ROS_WARN(
+        "The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.",
+        topic_name_.c_str(), (ros::Time::now() - last_updated_).toSec(), expected_update_rate_.toSec());
+  }
+  return current;
+}
+
+void ObservationBuffer::resetLastUpdated()
+{
+  last_updated_ = ros::Time::now();
+}
+}  // namespace costmap_2d
+