1726_24102025

plugins/critical_layer.cpp

@@ -0,0 +1,38 @@
+#include <costmap_2d/critical_layer.h>
+#include <boost/dll/alias.hpp>
+#include "shared.h"
+
+PLUGINLIB_EXPORT_CLASS(costmap_2d::CriticalLayer, costmap_2d::Layer)
+
+using costmap_2d::NO_INFORMATION;
+using costmap_2d::LETHAL_OBSTACLE;
+using costmap_2d::CRITICAL_SPACE;
+
+namespace costmap_2d
+{
+
+CriticalLayer::CriticalLayer(){}
+CriticalLayer::~CriticalLayer(){}
+
+unsigned char CriticalLayer::interpretValue(unsigned char value) 
+{
+  // check if the static value is above the unknown or lethal thresholds
+  if(value >= *this->threshold_)
+    return CRITICAL_SPACE;
+  else 
+    return NO_INFORMATION;
+}
+
+void CriticalLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+{
+  if (!map_received_)
+    return;
+    
+    // if not rolling, the layered costmap (master_grid) has same coordinates as this layer
+    if (!use_maximum_)
+      updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); 
+    else
+      updateWithMax(master_grid, min_i, min_j, max_i, max_j);
+    
+}
+}

plugins/directional_layer.cpp

@@ -0,0 +1,412 @@
+#include <costmap_2d/directional_layer.h>
+#include <pluginlib/class_list_macros.hpp>
+#include <tf/transform_listener.h>
+#include <tf2/convert.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf/transform_datatypes.h>
+
+PLUGINLIB_EXPORT_CLASS(costmap_2d::DirectionalLayer, costmap_2d::Layer)
+
+#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
+
+using costmap_2d::CRITICAL_SPACE;
+using costmap_2d::LETHAL_OBSTACLE;
+using costmap_2d::NO_INFORMATION;
+
+namespace costmap_2d
+{
+  DirectionalLayer::DirectionalLayer()
+  {
+    ros::NodeHandle nh("~/" + name_);
+    lane_mask_pub_ = nh.advertise<nav_msgs::OccupancyGrid>("/direction_zones/lanes", 1);
+  }
+  DirectionalLayer::~DirectionalLayer() {}
+
+  bool DirectionalLayer::laneFilter(const std::vector<geometry_msgs::PoseStamped> plan)
+  {
+    if (directional_areas_.empty())
+      throw "Has no map data";
+    nav_msgs::Path path;
+    path.header.stamp = ros::Time::now();
+    path.header.frame_id = map_frame_;
+    path.poses = plan;
+    return this->laneFilter(directional_areas_, path);
+  }
+
+  void DirectionalLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr &new_map)
+  {
+    unsigned int size_x = new_map->info.width, size_y = new_map->info.height;
+
+    ROS_DEBUG("Received a %d X %d map at %f m/pix", size_x / 2, size_y / 2, new_map->info.resolution);
+
+    // resize costmap if size, resolution or origin do not match
+    Costmap2D *master = layered_costmap_->getCostmap();
+    if (!layered_costmap_->isRolling() &&
+        (master->getSizeInCellsX() != size_x / 2 ||
+         master->getSizeInCellsY() != size_y / 2 ||
+         master->getResolution() != new_map->info.resolution ||
+         master->getOriginX() != new_map->info.origin.position.x ||
+         master->getOriginY() != new_map->info.origin.position.y))
+    {
+      // Update the size of the layered costmap (and all layers, including this one)
+      ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x / 2, size_y / 2, new_map->info.resolution);
+      layered_costmap_->resizeMap(size_x / 2, size_y / 2, new_map->info.resolution, new_map->info.origin.position.x,
+                                  new_map->info.origin.position.y,
+                                  true /* set size_locked to true, prevents reconfigureCb from overriding map size*/);
+    }
+    else if (size_x_ != size_x / 2 || size_y_ != size_y / 2 ||
+             resolution_ != new_map->info.resolution ||
+             origin_x_ != new_map->info.origin.position.x ||
+             origin_y_ != new_map->info.origin.position.y)
+    {
+      // only update the size of the costmap stored locally in this layer
+      ROS_INFO("Resizing static layer to %d X %d at %f m/pix", size_x / 2, size_y / 2, new_map->info.resolution);
+      resizeMap(size_x / 2, size_y / 2, new_map->info.resolution,
+                new_map->info.origin.position.x, new_map->info.origin.position.y);
+    }
+    unsigned char values[4];
+    directional_areas_.clear();
+    directional_areas_.resize(size_x / 2 * size_y / 2);
+    // initialize the costmap with static data
+    for (unsigned int iy = 0; iy < size_y; iy += 2)
+    {
+      for (unsigned int ix = 0; ix < size_x; ix += 2)
+      {
+        values[0] = new_map->data[MAP_IDX(size_x, ix, iy)];
+        values[1] = new_map->data[MAP_IDX(size_x, ix + 1, iy)];
+        values[2] = new_map->data[MAP_IDX(size_x, ix, iy + 1)];
+        values[3] = new_map->data[MAP_IDX(size_x, ix + 1, iy + 1)];
+        uint32_t color_avg = (uint32_t)(values[0] | values[1] << 8u | values[2] << 16u | values[3] << 24u);
+        int index = getIndex(ix / 2, iy / 2);
+        directional_areas_[index][0] = (uint16_t)(color_avg & 0xfff);
+        directional_areas_[index][1] = (uint16_t)(color_avg >> 12u);
+        costmap_[index] = costmap_2d::NO_INFORMATION;
+      }
+    }
+    map_frame_ = new_map->header.frame_id;
+
+    // we have a new map, update full size of map
+    x_ = y_ = 0;
+    width_ = size_x_;
+    height_ = size_y_;
+    map_received_ = true;
+    has_updated_data_ = true;
+
+    new_map_.header = new_map->header;
+    new_map_.header.stamp = ros::Time::now();
+    new_map_.info = new_map->info;
+    new_map_.info.width = width_;
+    new_map_.info.height = height_;
+
+    // shutdown the map subscrber if firt_map_only_ flag is on
+    if (first_map_only_)
+    {
+      ROS_INFO("Shutting down the map subscriber. first_map_only flag is on");
+      map_sub_.shutdown();
+    }
+  }
+
+  bool DirectionalLayer::laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const nav_msgs::Path path)
+  {
+    boost::unique_lock<mutex_t> lock(*layered_costmap_->getCostmap()->getMutex());
+    if (new_map.empty())
+      return false;
+
+    std::vector<std::pair<unsigned int, double>> X_List, Y_List;
+    std::vector<double> Yaw_list;
+
+    const geometry_msgs::PoseStamped &e = path.poses.back();
+    
+    bool get_success = getRobotPose(pose_x_, pose_y_, pose_yaw_);
+    if(!get_success) return false;
+    for (auto it = path.poses.begin(); it != path.poses.end(); ++it)
+    {
+      const geometry_msgs::PoseStamped &p = *it; // Get the pose from the iterator
+      unsigned int mx, my;
+      if (!worldToMap(p.pose.position.x, p.pose.position.y, mx, my))
+      {
+        ROS_ERROR("ERROR: trying to set a start cell %f %f that is outside of map\n", p.pose.position.x, p.pose.position.y);
+        return false;
+      }
+      // Convert to yaw
+      tf::Quaternion quaternion;
+      tf::quaternionMsgToTF(p.pose.orientation, quaternion);
+      double theta = tf::getYaw(quaternion);
+      unsigned char value = laneFilter(mx, my, theta);
+      if (get_success)
+      {
+        if (
+            (!inSkipErea(pose_x_, pose_y_, p.pose.position.x, p.pose.position.y, distance_skip_) & 
+            !inSkipErea(p.pose.position.x, p.pose.position.y, e.pose.position.x, e.pose.position.y, distance_skip_))
+            || p == path.poses.back()
+           )
+        {
+          if (value == costmap_2d::INSCRIBED_INFLATED_OBSTACLE || value == costmap_2d::LETHAL_OBSTACLE)
+          {
+            std::pair<unsigned int, double> x_val(mx, p.pose.position.x);
+            std::pair<unsigned int, double> y_val(my, p.pose.position.y);
+            X_List.push_back(x_val);
+            Y_List.push_back(y_val);
+            Yaw_list.push_back(theta);
+            // costmap_[getIndex(mx, my)] = value;
+          }
+        }
+      }
+    }
+    if (!Yaw_list.empty())
+    {
+      laneFilter(X_List, Y_List, Yaw_list);
+      nav_msgs::OccupancyGrid lanes;
+      convertToMap(costmap_, lanes, 0.65, 0.196);
+      lane_mask_pub_.publish(lanes);
+      return false;
+    }
+    return true;
+    
+  }
+
+  void DirectionalLayer::convertToMap(unsigned char *costmap, nav_msgs::OccupancyGrid &dir_map, double occ_th, double free_th)
+  {
+    dir_map.header = new_map_.header;
+    dir_map.header.stamp = ros::Time::now();
+    dir_map.info = new_map_.info;
+    int size_costmap = new_map_.info.width * new_map_.info.height;
+    dir_map.data.resize(size_costmap);
+    for (int i = 0; i < size_costmap; i++)
+    {
+      int8_t value;
+      if (costmap[i] == costmap_2d::NO_INFORMATION)
+      {
+        value = -1;
+      }
+      else
+      {
+        double occ = (costmap[i]) / 255.0;
+        if (occ > occ_th)
+          value = +100;
+        else if (occ < free_th)
+          value = 0;
+        else
+        {
+          double ratio = (occ - free_th) / (occ_th - free_th);
+          value = 1 + 98 * ratio;
+        }
+      }
+      dir_map.data[i] = value;
+    }
+  }
+
+  unsigned char DirectionalLayer::laneFilter(unsigned int x, unsigned int y, double yaw_robot)
+  {
+    double yaw_lane;
+    unsigned char cost = costmap_2d::NO_INFORMATION;
+
+    int index = getIndex(x, y);
+    for (auto &lane : directional_areas_[index])
+    {
+      if (lane > 359)
+      {
+        cost = std::min(cost, costmap_2d::NO_INFORMATION);
+      }
+      else
+      {
+        double yaw_lane = (double)lane / 180 * M_PI;
+        if (0.4 <= cos(yaw_robot - yaw_lane) && cos(yaw_robot - yaw_lane) <= 1.0)
+          cost = std::min(cost, costmap_2d::FREE_SPACE);
+        else if (-1.0 <= cos(yaw_robot - yaw_lane) && cos(yaw_robot - yaw_lane) <= -0.4)
+          cost = std::min(cost, costmap_2d::LETHAL_OBSTACLE);
+        else
+          cost = std::min(cost, costmap_2d::NO_INFORMATION);
+      }
+    }
+    return cost;
+  }
+
+  void DirectionalLayer::laneFilter(std::vector<std::pair<unsigned int, double>> x, 
+                                    std::vector<std::pair<unsigned int, double>> y, 
+                                    std::vector<double> yaw_robot)
+  {
+    if(x.empty() || y.empty() || yaw_robot.empty()) 
+      return;
+
+    unsigned int x_min, y_min, x_max, y_max;
+    double x_min_w, y_min_w, x_max_w, y_max_w;
+
+    x_min = x.front().first; y_min = y.front().first;
+    x_max = x.front().first; y_max = y.front().first;
+    x_min_w = x.front().second; y_min_w = y.front().second;
+    x_max_w = x.front().second; y_max_w = y.front().second;
+    for (int i = 1; i < yaw_robot.size(); i++)
+    {
+      if(inSkipErea(pose_x_, pose_y_, x[i].second, y[i].second, distance_skip_))
+        continue;
+      x_min = std::min(x_min, x[i].first);
+      y_min = std::min(y_min, y[i].first);
+      x_max = std::max(x_max, x[i].first);
+      y_max = std::max(y_max, y[i].first);
+
+      x_min_w = std::min(x_min_w, x[i].second);
+      y_min_w = std::min(y_min_w, y[i].second);
+      x_max_w = std::max(x_max_w, x[i].second);
+      y_max_w = std::max(y_max_w, y[i].second);
+
+    }
+    // ROS_INFO("%d %d %d %d", x_min, y_min, x_max, y_max);
+    // ROS_INFO("%f %f %f %f", x_min_w, y_min_w, x_max_w, y_max_w);
+    for (int i = 0; i < yaw_robot.size(); i++)
+    {
+      if(inSkipErea(pose_x_, pose_y_, x[i].second, y[i].second, distance_skip_))
+        continue;
+
+      double yaw_rad = (double)yaw_robot[i] / 180 * M_PI;
+      if (fabs(cos(yaw_rad)) > fabs(sin(yaw_rad)))
+      {
+        int x_ = x[i].first;
+        // Dưới lên trên
+        for (int j = y[i].first; j <= y_max; j++)
+        {
+          int y_ = j;
+          if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
+            continue;
+          unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
+          if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
+            costmap_[getIndex(x_, y_)] = value;
+          else
+            break;
+        }
+        // Trên xuống dưới
+        for (int k = y[i].first; k >= y_min; k--)
+        {
+          int y_ = k;
+          if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
+            continue;
+          unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
+          if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
+            costmap_[getIndex(x_, y_)] = value;
+          else
+            break;
+        }
+
+        int y_ = y[i].first;
+        // Phải qua trái
+        for (int j = x[i].first; j < layered_costmap_->getCostmap()->getSizeInCellsX(); j++)
+        {
+          int x_ = j;
+          if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
+            continue;
+          unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
+          if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
+            costmap_[getIndex(x_, y_)] = value;
+          else
+            break;
+        }
+        // Trái qua phải
+        for (int k = x[i].first; k >= 0; k--)
+        {
+          int x_ = k;
+          if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
+            continue;
+          unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
+          if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
+            costmap_[getIndex(x_, y_)] = value;
+          else
+            break;
+        }       
+      }
+      else if(fabs(cos(yaw_rad)) < fabs(sin(yaw_rad)))
+      {
+        int y_ = y[i].first;
+        // Phải qua trái
+        for (int j = x[i].first; j <= x_max; j++)
+        {
+          int x_ = j;
+          if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
+            continue;
+          unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
+          if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
+            costmap_[getIndex(x_, y_)] = value;
+          else
+            break;
+        }
+        // Trái qua phải
+        for (int k = x[i].first; k >= x_min; k--)
+        {
+          int x_ = k;
+          if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
+            continue;
+          unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
+          if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
+            costmap_[getIndex(x_, y_)] = value;
+          else
+            break;
+        }
+
+        int x_ = x[i].first;
+        // Dưới lên trên
+        for (int j = y[i].first; j < layered_costmap_->getCostmap()->getSizeInCellsY(); j++)
+        {
+          int y_ = j;
+          if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
+            continue;
+          unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
+          if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
+            costmap_[getIndex(x_, y_)] = value;
+          else
+            break;
+        }
+        // Trên xuống dưới
+        for (int k = y[i].first; k >= 0; k--)
+        {
+          int y_ = k;
+          if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
+            continue;
+          unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
+          if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
+            costmap_[getIndex(x_, y_)] = value;
+          else
+            break;
+        }        
+      }
+    }
+  }
+
+  void DirectionalLayer::resetMap()
+  {
+    unsigned int size_x = layered_costmap_->getCostmap()->getSizeInCellsX();
+    unsigned int size_y = layered_costmap_->getCostmap()->getSizeInCellsY();
+    for (unsigned int iy = 0; iy < size_y; iy++)
+    {
+      for (unsigned int ix = 0; ix < size_x; ix++)
+      {
+        int index = getIndex(ix, iy);
+        costmap_[index] = costmap_2d::NO_INFORMATION;
+      }
+    }
+  }
+
+  bool DirectionalLayer::getRobotPose(double &x, double &y, double &yaw)
+  {
+    tf::StampedTransform transform;
+    try
+    {
+      listener_.lookupTransform(map_frame_, base_frame_id_, ros::Time(0), transform);
+    }
+    catch (tf::TransformException &ex)
+    {
+      ROS_ERROR("%s", ex.what());
+      return false;
+    }
+    x = transform.getOrigin().x();
+    y = transform.getOrigin().y();
+    // Extract the rotation as a quaternion from the transform
+    tf::Quaternion rotation = transform.getRotation();
+    // Convert the quaternion to a yaw angle (in radians)
+    yaw = tf::getYaw(rotation);
+    return true;
+  }
+
+  bool DirectionalLayer::inSkipErea(double start_x, double start_y, double end_x, double end_y, double skip_distance)
+  {
+    return fabs(hypot(start_x - end_x, start_y - end_y)) <= skip_distance;
+  }
+}

plugins/obstacle_layer.cpp

@@ -0,0 +1,623 @@
+/*********************************************************************
+ *
+ * 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
+ *         David V. Lu!!
+ *********************************************************************/
+#include <costmap_2d/obstacle_layer.h>
+#include <costmap_2d/costmap_math.h>
+#include <tf2_ros/message_filter.h>
+
+#include <pluginlib/class_list_macros.hpp>
+#include <sensor_msgs/point_cloud2_iterator.h>
+
+PLUGINLIB_EXPORT_CLASS(costmap_2d::ObstacleLayer, costmap_2d::Layer)
+
+using costmap_2d::NO_INFORMATION;
+using costmap_2d::LETHAL_OBSTACLE;
+using costmap_2d::FREE_SPACE;
+
+using costmap_2d::ObservationBuffer;
+using costmap_2d::Observation;
+
+namespace costmap_2d
+{
+
+void ObstacleLayer::onInitialize()
+{
+  ros::NodeHandle nh("~/" + name_), g_nh;
+  rolling_window_ = layered_costmap_->isRolling();
+
+  bool track_unknown_space;
+  nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown());
+  if (track_unknown_space)
+    default_value_ = NO_INFORMATION;
+  else
+    default_value_ = FREE_SPACE;
+
+  ObstacleLayer::matchSize();
+  current_ = true;
+
+  global_frame_ = layered_costmap_->getGlobalFrameID();
+  double transform_tolerance;
+  nh.param("transform_tolerance", transform_tolerance, 0.2);
+
+  std::string topics_string;
+  // get the topics that we'll subscribe to from the parameter server
+  nh.param("observation_sources", topics_string, std::string(""));
+  ROS_INFO("    Subscribed to Topics: %s", topics_string.c_str());
+
+  // now we need to split the topics based on whitespace which we can use a stringstream for
+  std::stringstream ss(topics_string);
+
+  std::string source;
+  while (ss >> source)
+  {
+    ros::NodeHandle source_node(nh, source);
+
+    // get the parameters for the specific topic
+    double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height;
+    std::string topic, sensor_frame, data_type;
+    bool inf_is_valid, clearing, marking;
+
+    source_node.param("topic", topic, source);
+    source_node.param("sensor_frame", sensor_frame, std::string(""));
+    source_node.param("observation_persistence", observation_keep_time, 0.0);
+    source_node.param("expected_update_rate", expected_update_rate, 0.0);
+    source_node.param("data_type", data_type, std::string("PointCloud"));
+    source_node.param("min_obstacle_height", min_obstacle_height, 0.0);
+    source_node.param("max_obstacle_height", max_obstacle_height, 2.0);
+    source_node.param("inf_is_valid", inf_is_valid, false);
+    source_node.param("clearing", clearing, false);
+    source_node.param("marking", marking, true);
+
+    if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan"))
+    {
+      ROS_FATAL("Only topics that use point clouds or laser scans are currently supported");
+      throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported");
+    }
+
+    std::string raytrace_range_param_name, obstacle_range_param_name;
+
+    // get the obstacle range for the sensor
+    double obstacle_range = 2.5;
+    if (source_node.searchParam("obstacle_range", obstacle_range_param_name))
+    {
+      source_node.getParam(obstacle_range_param_name, obstacle_range);
+    }
+
+    // get the raytrace range for the sensor
+    double raytrace_range = 3.0;
+    if (source_node.searchParam("raytrace_range", raytrace_range_param_name))
+    {
+      source_node.getParam(raytrace_range_param_name, raytrace_range);
+    }
+
+    ROS_DEBUG("Creating an observation buffer for source %s, topic %s, frame %s", source.c_str(), topic.c_str(),
+              sensor_frame.c_str());
+
+    // create an observation buffer
+    observation_buffers_.push_back(
+        boost::shared_ptr < ObservationBuffer
+            > (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,
+                                     max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_,
+                                     sensor_frame, transform_tolerance)));
+
+    // check if we'll add this buffer to our marking observation buffers
+    if (marking)
+      marking_buffers_.push_back(observation_buffers_.back());
+
+    // check if we'll also add this buffer to our clearing observation buffers
+    if (clearing)
+      clearing_buffers_.push_back(observation_buffers_.back());
+
+    ROS_DEBUG(
+        "Created an observation buffer for source %s, topic %s, global frame: %s, "
+        "expected update rate: %.2f, observation persistence: %.2f",
+        source.c_str(), topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);
+
+    // create a callback for the topic
+    if (data_type == "LaserScan")
+    {
+      boost::shared_ptr < message_filters::Subscriber<sensor_msgs::LaserScan>
+          > sub(new message_filters::Subscriber<sensor_msgs::LaserScan>(g_nh, topic, 50));
+
+      boost::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::LaserScan> > filter(
+        new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*sub, *tf_, global_frame_, 50, g_nh));
+
+      if (inf_is_valid)
+      {
+        filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ laserScanValidInfCallback(msg, buffer); });
+      }
+      else
+      {
+        filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ laserScanCallback(msg, buffer); });
+      }
+
+      observation_subscribers_.push_back(sub);
+      observation_notifiers_.push_back(filter);
+
+      observation_notifiers_.back()->setTolerance(ros::Duration(0.05));
+    }
+    else if (data_type == "PointCloud")
+    {
+      boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud>
+          > sub(new message_filters::Subscriber<sensor_msgs::PointCloud>(g_nh, topic, 50));
+
+      if (inf_is_valid)
+      {
+       ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
+      }
+
+        boost::shared_ptr < tf2_ros::MessageFilter<sensor_msgs::PointCloud>
+        > filter(new tf2_ros::MessageFilter<sensor_msgs::PointCloud>(*sub, *tf_, global_frame_, 50, g_nh));
+        filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ pointCloudCallback(msg, buffer); });
+
+      observation_subscribers_.push_back(sub);
+      observation_notifiers_.push_back(filter);
+    }
+    else
+    {
+      boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud2>
+          > sub(new message_filters::Subscriber<sensor_msgs::PointCloud2>(g_nh, topic, 50));
+
+      if (inf_is_valid)
+      {
+       ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
+      }
+
+      boost::shared_ptr < tf2_ros::MessageFilter<sensor_msgs::PointCloud2>
+      > filter(new tf2_ros::MessageFilter<sensor_msgs::PointCloud2>(*sub, *tf_, global_frame_, 50, g_nh));
+      filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ pointCloud2Callback(msg, buffer); });
+
+      observation_subscribers_.push_back(sub);
+      observation_notifiers_.push_back(filter);
+    }
+
+    if (sensor_frame != "")
+    {
+      std::vector < std::string > target_frames;
+      target_frames.push_back(global_frame_);
+      target_frames.push_back(sensor_frame);
+      observation_notifiers_.back()->setTargetFrames(target_frames);
+    }
+  }
+
+  dsrv_ = NULL;
+  setupDynamicReconfigure(nh);
+}
+
+void ObstacleLayer::setupDynamicReconfigure(ros::NodeHandle& nh)
+{
+  dsrv_ = new dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>(nh);
+  dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>::CallbackType cb =
+      [this](auto& config, auto level){ reconfigureCB(config, level); };
+  dsrv_->setCallback(cb);
+}
+
+ObstacleLayer::~ObstacleLayer()
+{
+    if (dsrv_)
+        delete dsrv_;
+}
+void ObstacleLayer::reconfigureCB(costmap_2d::ObstaclePluginConfig &config, uint32_t level)
+{
+  enabled_ = config.enabled;
+  footprint_clearing_enabled_ = config.footprint_clearing_enabled;
+  max_obstacle_height_ = config.max_obstacle_height;
+  combination_method_ = config.combination_method;
+}
+
+void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScanConstPtr& message,
+                                      const boost::shared_ptr<ObservationBuffer>& buffer)
+{
+  // project the laser into a point cloud
+  sensor_msgs::PointCloud2 cloud;
+  cloud.header = message->header;
+
+  // project the scan into a point cloud
+  try
+  {
+    projector_.transformLaserScanToPointCloud(message->header.frame_id, *message, cloud, *tf_);
+  }
+  catch (tf2::TransformException &ex)
+  {
+    ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", global_frame_.c_str(),
+             ex.what());
+    projector_.projectLaser(*message, cloud);
+  }
+  catch (std::runtime_error &ex)
+  {
+    ROS_WARN("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s", ex.what());
+    return; //ignore this message
+  }
+
+  // buffer the point cloud
+  buffer->lock();
+  buffer->bufferCloud(cloud);
+  buffer->unlock();
+}
+
+void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr& raw_message,
+                                              const boost::shared_ptr<ObservationBuffer>& buffer)
+{
+  // Filter positive infinities ("Inf"s) to max_range.
+  float epsilon = 0.0001;  // a tenth of a millimeter
+  sensor_msgs::LaserScan message = *raw_message;
+  for (size_t i = 0; i < message.ranges.size(); i++)
+  {
+    float range = message.ranges[ i ];
+    if (!std::isfinite(range) && range > 0)
+    {
+      message.ranges[ i ] = message.range_max - epsilon;
+    }
+  }
+
+  // project the laser into a point cloud
+  sensor_msgs::PointCloud2 cloud;
+  cloud.header = message.header;
+
+  // project the scan into a point cloud
+  try
+  {
+    projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_);
+  }
+  catch (tf2::TransformException &ex)
+  {
+    ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s",
+             global_frame_.c_str(), ex.what());
+    projector_.projectLaser(message, cloud);
+  }
+  catch (std::runtime_error &ex)
+  {
+    ROS_WARN("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s", ex.what());
+    return; //ignore this message
+  }
+
+  // buffer the point cloud
+  buffer->lock();
+  buffer->bufferCloud(cloud);
+  buffer->unlock();
+}
+
+void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloudConstPtr& message,
+                                               const boost::shared_ptr<ObservationBuffer>& buffer)
+{
+  sensor_msgs::PointCloud2 cloud2;
+
+  if (!sensor_msgs::convertPointCloudToPointCloud2(*message, cloud2))
+  {
+    ROS_ERROR("Failed to convert a PointCloud to a PointCloud2, dropping message");
+    return;
+  }
+
+  // buffer the point cloud
+  buffer->lock();
+  buffer->bufferCloud(cloud2);
+  buffer->unlock();
+}
+
+void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& message,
+                                                const boost::shared_ptr<ObservationBuffer>& buffer)
+{
+  // buffer the point cloud
+  buffer->lock();
+  buffer->bufferCloud(*message);
+  buffer->unlock();
+}
+
+void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
+                                          double* min_y, double* max_x, double* max_y)
+{
+  if (rolling_window_)
+    updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
+  useExtraBounds(min_x, min_y, max_x, max_y);
+
+  bool current = true;
+  std::vector<Observation> observations, clearing_observations;
+
+  // get the marking observations
+  current = current && getMarkingObservations(observations);
+
+  // get the clearing observations
+  current = current && getClearingObservations(clearing_observations);
+
+  // update the global current status
+  current_ = current;
+
+  // raytrace freespace
+  for (unsigned int i = 0; i < clearing_observations.size(); ++i)
+  {
+    raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
+  }
+
+  // place the new obstacles into a priority queue... each with a priority of zero to begin with
+  for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
+  {
+    const Observation& obs = *it;
+
+    const sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
+
+    double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
+
+    sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
+    sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
+    sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
+
+    for (; iter_x !=iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
+    {
+      double px = *iter_x, py = *iter_y, pz = *iter_z;
+
+      // if the obstacle is too high or too far away from the robot we won't add it
+      if (pz > max_obstacle_height_)
+      {
+        ROS_DEBUG("The point is too high");
+        continue;
+      }
+
+      // compute the squared distance from the hitpoint to the pointcloud's origin
+      double sq_dist = (px - obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y)
+          + (pz - obs.origin_.z) * (pz - obs.origin_.z);
+
+      // if the point is far enough away... we won't consider it
+      if (sq_dist >= sq_obstacle_range)
+      {
+        ROS_DEBUG("The point is too far away");
+        continue;
+      }
+
+      // now we need to compute the map coordinates for the observation
+      unsigned int mx, my;
+      if (!worldToMap(px, py, mx, my))
+      {
+        ROS_DEBUG("Computing map coords failed");
+        continue;
+      }
+
+      unsigned int index = getIndex(mx, my);
+      costmap_[index] = LETHAL_OBSTACLE;
+      touch(px, py, min_x, min_y, max_x, max_y);
+    }
+  }
+
+  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
+}
+
+void ObstacleLayer::updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
+                                    double* max_x, double* max_y)
+{
+    if (!footprint_clearing_enabled_) return;
+    transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_);
+
+    for (unsigned int i = 0; i < transformed_footprint_.size(); i++)
+    {
+      touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y);
+    }
+}
+
+void ObstacleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+{
+  if (footprint_clearing_enabled_)
+  {
+    setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE);
+  }
+  
+  switch (combination_method_)
+  {
+    case 0:  // Overwrite
+      updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j);
+      break;
+    case 1:  // Maximum
+      updateWithMax(master_grid, min_i, min_j, max_i, max_j);
+      break;
+    default:  // Nothing
+      break;
+  }
+
+}
+
+void ObstacleLayer::addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing)
+{
+  if (marking)
+    static_marking_observations_.push_back(obs);
+  if (clearing)
+    static_clearing_observations_.push_back(obs);
+}
+
+void ObstacleLayer::clearStaticObservations(bool marking, bool clearing)
+{
+  if (marking)
+    static_marking_observations_.clear();
+  if (clearing)
+    static_clearing_observations_.clear();
+}
+
+bool ObstacleLayer::getMarkingObservations(std::vector<Observation>& marking_observations) const
+{
+  bool current = true;
+  // get the marking observations
+  for (unsigned int i = 0; i < marking_buffers_.size(); ++i)
+  {
+    marking_buffers_[i]->lock();
+    marking_buffers_[i]->getObservations(marking_observations);
+    current = marking_buffers_[i]->isCurrent() && current;
+    marking_buffers_[i]->unlock();
+  }
+  marking_observations.insert(marking_observations.end(),
+                              static_marking_observations_.begin(), static_marking_observations_.end());
+  return current;
+}
+
+bool ObstacleLayer::getClearingObservations(std::vector<Observation>& clearing_observations) const
+{
+  bool current = true;
+  // get the clearing observations
+  for (unsigned int i = 0; i < clearing_buffers_.size(); ++i)
+  {
+    clearing_buffers_[i]->lock();
+    clearing_buffers_[i]->getObservations(clearing_observations);
+    current = clearing_buffers_[i]->isCurrent() && current;
+    clearing_buffers_[i]->unlock();
+  }
+  clearing_observations.insert(clearing_observations.end(),
+                              static_clearing_observations_.begin(), static_clearing_observations_.end());
+  return current;
+}
+
+void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
+                                              double* max_x, double* max_y)
+{
+  double ox = clearing_observation.origin_.x;
+  double oy = clearing_observation.origin_.y;
+  const sensor_msgs::PointCloud2 &cloud = *(clearing_observation.cloud_);
+
+  // get the map coordinates of the origin of the sensor
+  unsigned int x0, y0;
+  if (!worldToMap(ox, oy, x0, y0))
+  {
+    ROS_WARN_THROTTLE(
+        1.0, "The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
+        ox, oy);
+    return;
+  }
+
+  // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
+  double origin_x = origin_x_, origin_y = origin_y_;
+  double map_end_x = origin_x + size_x_ * resolution_;
+  double map_end_y = origin_y + size_y_ * resolution_;
+
+
+  touch(ox, oy, min_x, min_y, max_x, max_y);
+
+  // for each point in the cloud, we want to trace a line from the origin and clear obstacles along it
+  sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
+
+  for (; iter_x != iter_x.end(); ++iter_x, ++iter_y)
+  {
+    double wx = *iter_x;
+    double wy = *iter_y;
+
+    // now we also need to make sure that the enpoint we're raytracing
+    // to isn't off the costmap and scale if necessary
+    double a = wx - ox;
+    double b = wy - oy;
+
+    // the minimum value to raytrace from is the origin
+    if (wx < origin_x)
+    {
+      double t = (origin_x - ox) / a;
+      wx = origin_x;
+      wy = oy + b * t;
+    }
+    if (wy < origin_y)
+    {
+      double t = (origin_y - oy) / b;
+      wx = ox + a * t;
+      wy = origin_y;
+    }
+
+    // the maximum value to raytrace to is the end of the map
+    if (wx > map_end_x)
+    {
+      double t = (map_end_x - ox) / a;
+      wx = map_end_x - .001;
+      wy = oy + b * t;
+    }
+    if (wy > map_end_y)
+    {
+      double t = (map_end_y - oy) / b;
+      wx = ox + a * t;
+      wy = map_end_y - .001;
+    }
+
+    // now that the vector is scaled correctly... we'll get the map coordinates of its endpoint
+    unsigned int x1, y1;
+
+    // check for legality just in case
+    if (!worldToMap(wx, wy, x1, y1))
+      continue;
+
+    unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
+    MarkCell marker(costmap_, FREE_SPACE);
+    // and finally... we can execute our trace to clear obstacles along that line
+    raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range);
+
+    updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
+  }
+}
+
+void ObstacleLayer::activate()
+{
+  // if we're stopped we need to re-subscribe to topics
+  for (unsigned int i = 0; i < observation_subscribers_.size(); ++i)
+  {
+    if (observation_subscribers_[i] != NULL)
+      observation_subscribers_[i]->subscribe();
+  }
+
+  for (unsigned int i = 0; i < observation_buffers_.size(); ++i)
+  {
+    if (observation_buffers_[i])
+      observation_buffers_[i]->resetLastUpdated();
+  }
+}
+void ObstacleLayer::deactivate()
+{
+  for (unsigned int i = 0; i < observation_subscribers_.size(); ++i)
+  {
+    if (observation_subscribers_[i] != NULL)
+      observation_subscribers_[i]->unsubscribe();
+  }
+}
+
+void ObstacleLayer::updateRaytraceBounds(double ox, double oy, double wx, double wy, double range,
+                                         double* min_x, double* min_y, double* max_x, double* max_y)
+{
+  double dx = wx-ox, dy = wy-oy;
+  double full_distance = hypot(dx, dy);
+  double scale = std::min(1.0, range / full_distance);
+  double ex = ox + dx * scale, ey = oy + dy * scale;
+  touch(ex, ey, min_x, min_y, max_x, max_y);
+}
+
+void ObstacleLayer::reset()
+{
+    deactivate();
+    resetMaps();
+    current_ = true;
+    activate();
+}
+
+}  // namespace costmap_2d

plugins/preferred_layer.cpp

@@ -0,0 +1,28 @@
+#include <costmap_2d/preferred_layer.h>
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(costmap_2d::PreferredLayer, costmap_2d::Layer)
+
+using costmap_2d::NO_INFORMATION;
+using costmap_2d::FREE_SPACE;
+using costmap_2d::LETHAL_OBSTACLE;
+using costmap_2d::PREFERRED_SPACE;
+
+namespace costmap_2d
+{
+
+PreferredLayer::PreferredLayer(){}
+PreferredLayer::~PreferredLayer(){}
+
+unsigned char PreferredLayer::interpretValue(unsigned char value) 
+{
+  // check if the static value is above the unknown or lethal thresholds
+  if(value == 0) return NO_INFORMATION;
+  else if (value >= *this->threshold_)
+    return PREFERRED_SPACE;
+
+  double scale = (double) value / *this->threshold_;
+  return scale * LETHAL_OBSTACLE;
+}
+
+}

plugins/static_layer.cpp

@@ -0,0 +1,351 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, 2013, Willow Garage, Inc.
+ *  Copyright (c) 2015, Fetch Robotics, 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
+ *         David V. Lu!!
+ *********************************************************************/
+#include <costmap_2d/static_layer.h>
+#include <costmap_2d/costmap_math.h>
+#include <pluginlib/class_list_macros.hpp>
+#include <tf2/convert.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+PLUGINLIB_EXPORT_CLASS(costmap_2d::StaticLayer, costmap_2d::Layer)
+
+using costmap_2d::NO_INFORMATION;
+using costmap_2d::LETHAL_OBSTACLE;
+using costmap_2d::FREE_SPACE;
+
+namespace costmap_2d
+{
+
+StaticLayer::StaticLayer() : dsrv_(NULL) 
+{
+  threshold_ = &lethal_threshold_;
+}
+
+StaticLayer::~StaticLayer()
+{
+  if (dsrv_)
+    delete dsrv_;
+}
+
+void StaticLayer::onInitialize()
+{
+  ros::NodeHandle nh("~/" + name_), g_nh;
+  current_ = true;
+
+  global_frame_ = layered_costmap_->getGlobalFrameID();
+  nh.param("map_topic", map_topic_, std::string("map")); 
+  nh.param("first_map_only", first_map_only_, false);
+  nh.param("subscribe_to_updates", subscribe_to_updates_, false);
+
+  nh.param("track_unknown_space", track_unknown_space_, true);
+  nh.param("use_maximum", use_maximum_, false);
+
+  int temp_lethal_threshold, temp_unknown_cost_value;
+  nh.param("lethal_cost_threshold", temp_lethal_threshold, int(100));
+  nh.param("unknown_cost_value", temp_unknown_cost_value, int(-1));
+  nh.param("trinary_costmap", trinary_costmap_, true);
+  nh.param("base_frame_id", base_frame_id_, std::string("base_footprint"));
+
+  // ROS_WARN("global_frame_[%s] map_frame_[%s] map_topic[%s] first_map_only[%d] subscribe_to_updates[%d] track_unknown_space[%d] use_maximum[%d] \n lethal_cost_threshold[%d] unknown_cost_value[%d] trinary_costmap[%d]",
+  // global_frame_.c_str(), map_frame_.c_str(),map_topic_.c_str(), first_map_only_, subscribe_to_updates_, track_unknown_space_, use_maximum_, temp_lethal_threshold, temp_unknown_cost_value, trinary_costmap_);  
+  lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
+  unknown_cost_value_ = temp_unknown_cost_value;
+
+  // Only resubscribe if topic has changed
+  if (map_sub_.getTopic() != ros::names::resolve(map_topic_))
+  {
+    // we'll subscribe to the latched topic that the map server uses
+    ROS_INFO("Requesting the map...");
+    map_sub_ = g_nh.subscribe(map_topic_, 1, &StaticLayer::incomingMap, this);
+    map_received_ = false;
+    has_updated_data_ = false;
+
+    ros::Rate r(10);
+    while (!map_received_ && g_nh.ok())
+    {
+      ros::spinOnce();
+      r.sleep();
+    }
+
+    ROS_INFO("Received a %d X %d map at %f m/pix", getSizeInCellsX(), getSizeInCellsY(), getResolution());
+
+    if (subscribe_to_updates_)
+    {
+      ROS_INFO("Subscribing to updates");
+      map_update_sub_ = g_nh.subscribe(map_topic_ + "_updates", 10, &StaticLayer::incomingUpdate, this);
+    }
+  }
+  else
+  {
+    has_updated_data_ = true;
+  }
+
+  if (dsrv_)
+  {
+    delete dsrv_;
+  }
+
+  dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
+  dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb =
+      [this](auto& config, auto level){ reconfigureCB(config, level); };
+  dsrv_->setCallback(cb);
+}
+
+void StaticLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
+{
+  if (config.enabled != enabled_)
+  {
+    enabled_ = config.enabled;
+    has_updated_data_ = true;
+    x_ = y_ = 0;
+    width_ = size_x_;
+    height_ = size_y_;
+  }
+}
+
+void StaticLayer::matchSize()
+{
+  // If we are using rolling costmap, the static map size is
+  //   unrelated to the size of the layered costmap
+  if (!layered_costmap_->isRolling())
+  {
+    Costmap2D* master = layered_costmap_->getCostmap();
+    resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
+              master->getOriginX(), master->getOriginY());
+  }
+}
+
+unsigned char StaticLayer::interpretValue(unsigned char value)
+{
+  // check if the static value is above the unknown or lethal thresholds
+  if (track_unknown_space_ && value == unknown_cost_value_)
+    return NO_INFORMATION;
+  else if (!track_unknown_space_ && value == unknown_cost_value_)
+    return FREE_SPACE;
+  else if (value >= lethal_threshold_)
+    return LETHAL_OBSTACLE;
+  else if (trinary_costmap_)
+    return FREE_SPACE;
+
+  double scale = (double) value / lethal_threshold_;
+  return scale * LETHAL_OBSTACLE;
+}
+
+void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map)
+{
+  unsigned int size_x = new_map->info.width, size_y = new_map->info.height;
+
+  ROS_DEBUG("Received a %d X %d map at %f m/pix", size_x, size_y, new_map->info.resolution);
+
+  // resize costmap if size, resolution or origin do not match
+  Costmap2D* master = layered_costmap_->getCostmap();
+  if (!layered_costmap_->isRolling() &&
+      (master->getSizeInCellsX() != size_x ||
+       master->getSizeInCellsY() != size_y ||
+       master->getResolution() != new_map->info.resolution ||
+       master->getOriginX() != new_map->info.origin.position.x ||
+       master->getOriginY() != new_map->info.origin.position.y))
+  {
+    // Update the size of the layered costmap (and all layers, including this one)
+    ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
+    layered_costmap_->resizeMap(size_x, size_y, new_map->info.resolution, new_map->info.origin.position.x,
+                                new_map->info.origin.position.y,
+                                true /* set size_locked to true, prevents reconfigureCb from overriding map size*/);
+  }
+  else if (size_x_ != size_x || size_y_ != size_y ||
+           resolution_ != new_map->info.resolution ||
+           origin_x_ != new_map->info.origin.position.x ||
+           origin_y_ != new_map->info.origin.position.y)
+  {
+    // only update the size of the costmap stored locally in this layer
+    ROS_INFO("Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
+    resizeMap(size_x, size_y, new_map->info.resolution,
+              new_map->info.origin.position.x, new_map->info.origin.position.y);
+  }
+
+  unsigned int index = 0;
+
+  // initialize the costmap with static data
+  for (unsigned int i = 0; i < size_y; ++i)
+  {
+    for (unsigned int j = 0; j < size_x; ++j)
+    {
+      unsigned char value = new_map->data[index];
+      costmap_[index] = interpretValue(value);
+      ++index;
+    }
+  }
+  map_frame_ = new_map->header.frame_id;
+
+  // we have a new map, update full size of map
+  x_ = y_ = 0;
+  width_ = size_x_;
+  height_ = size_y_;
+  map_received_ = true;
+  has_updated_data_ = true;
+
+  // shutdown the map subscrber if firt_map_only_ flag is on
+  if (first_map_only_)
+  {
+    ROS_INFO("Shutting down the map subscriber. first_map_only flag is on");
+    map_sub_.shutdown();
+  }
+}
+
+void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr& update)
+{
+  unsigned int di = 0;
+  for (unsigned int y = 0; y < update->height ; y++)
+  {
+    unsigned int index_base = (update->y + y) * size_x_;
+    for (unsigned int x = 0; x < update->width ; x++)
+    {
+      unsigned int index = index_base + x + update->x;
+      costmap_[index] = interpretValue(update->data[di++]);
+    }
+  }
+  x_ = update->x;
+  y_ = update->y;
+  width_ = update->width;
+  height_ = update->height;
+  has_updated_data_ = true;
+}
+
+void StaticLayer::activate()
+{
+  onInitialize();
+}
+
+void StaticLayer::deactivate()
+{
+  map_sub_.shutdown();
+  if (subscribe_to_updates_)
+    map_update_sub_.shutdown();
+}
+
+void StaticLayer::reset()
+{
+  if (first_map_only_)
+  {
+    has_updated_data_ = true;
+  }
+  else
+  {
+    onInitialize();
+  }
+}
+
+void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
+                               double* max_x, double* max_y)
+{
+
+  if( !layered_costmap_->isRolling() ){
+    if (!map_received_ || !(has_updated_data_ || has_extra_bounds_))
+      return;
+  }
+
+  useExtraBounds(min_x, min_y, max_x, max_y);
+
+  double wx, wy;
+
+  mapToWorld(x_, y_, wx, wy);
+  *min_x = std::min(wx, *min_x);
+  *min_y = std::min(wy, *min_y);
+
+  mapToWorld(x_ + width_, y_ + height_, wx, wy);
+  *max_x = std::max(wx, *max_x);
+  *max_y = std::max(wy, *max_y);
+
+  has_updated_data_ = false;
+}
+
+void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+{
+  if (!map_received_)
+    return;
+
+  if (!layered_costmap_->isRolling())
+  {
+    // if not rolling, the layered costmap (master_grid) has same coordinates as this layer
+    if (!use_maximum_)
+      updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); 
+    else
+      updateWithMax(master_grid, min_i, min_j, max_i, max_j);
+  }
+  else
+  {
+    // If rolling window, the master_grid is unlikely to have same coordinates as this layer
+    unsigned int mx, my;
+    double wx, wy;
+    // Might even be in a different frame
+    geometry_msgs::TransformStamped transform;
+    try
+    {
+      transform = tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0));
+    }
+    catch (tf2::TransformException ex)
+    {
+      ROS_ERROR("%s", ex.what());
+      return;
+    }
+    // Copy map data given proper transformations
+    tf2::Transform tf2_transform;
+    tf2::convert(transform.transform, tf2_transform);
+    for (unsigned int i = min_i; i < max_i; ++i)
+    {
+      for (unsigned int j = min_j; j < max_j; ++j)
+      {
+        // Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates
+        layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
+        // Transform from global_frame_ to map_frame_
+        tf2::Vector3 p(wx, wy, 0);
+        p = tf2_transform*p;
+        // Set master_grid with cell from map
+        if (worldToMap(p.x(), p.y(), mx, my))
+        {
+          if (!use_maximum_)
+            master_grid.setCost(i, j, getCost(mx, my));
+          else
+            master_grid.setCost(i, j, std::max(getCost(mx, my), master_grid.getCost(i, j)));
+        }
+      }
+    }
+  }
+}
+
+}  // namespace costmap_2d

plugins/unpreferred_layer.cpp

@@ -0,0 +1,27 @@
+#include <costmap_2d/unpreferred_layer.h>
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(costmap_2d::UnPreferredLayer, costmap_2d::Layer)
+
+using costmap_2d::NO_INFORMATION;
+using costmap_2d::LETHAL_OBSTACLE;
+using costmap_2d::PREFERRED_SPACE;
+
+namespace costmap_2d
+{
+
+UnPreferredLayer::UnPreferredLayer(){}
+UnPreferredLayer::~UnPreferredLayer(){}
+
+unsigned char UnPreferredLayer::interpretValue(unsigned char value) 
+{
+  // check if the static value is above the unknown or lethal thresholds
+  if(value == 0) return NO_INFORMATION;
+  else if (value >= *this->threshold_)
+    return UNPREFERRED_SPACE;
+
+  double scale = (double) value / *this->threshold_;
+  return scale * LETHAL_OBSTACLE;
+}
+
+}

plugins/voxel_layer.cpp

@@ -0,0 +1,449 @@
+/*********************************************************************
+ *
+ * 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
+ *         David V. Lu!!
+ *********************************************************************/
+#include <costmap_2d/voxel_layer.h>
+#include <pluginlib/class_list_macros.hpp>
+#include <sensor_msgs/point_cloud2_iterator.h>
+
+#define VOXEL_BITS 16
+PLUGINLIB_EXPORT_CLASS(costmap_2d::VoxelLayer, costmap_2d::Layer)
+
+using costmap_2d::NO_INFORMATION;
+using costmap_2d::LETHAL_OBSTACLE;
+using costmap_2d::FREE_SPACE;
+
+using costmap_2d::ObservationBuffer;
+using costmap_2d::Observation;
+
+namespace costmap_2d
+{
+
+void VoxelLayer::onInitialize()
+{
+  ObstacleLayer::onInitialize();
+  ros::NodeHandle private_nh("~/" + name_);
+
+  private_nh.param("publish_voxel_map", publish_voxel_, false);
+  if (publish_voxel_)
+    voxel_pub_ = private_nh.advertise < costmap_2d::VoxelGrid > ("voxel_grid", 1);
+
+  clearing_endpoints_pub_ = private_nh.advertise<sensor_msgs::PointCloud>("clearing_endpoints", 1);
+}
+
+void VoxelLayer::setupDynamicReconfigure(ros::NodeHandle& nh)
+{
+  voxel_dsrv_ = new dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>(nh);
+  dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>::CallbackType cb =
+      [this](auto& config, auto level){ reconfigureCB(config, level); };
+  voxel_dsrv_->setCallback(cb);
+}
+
+VoxelLayer::~VoxelLayer()
+{
+  if (voxel_dsrv_)
+    delete voxel_dsrv_;
+}
+
+void VoxelLayer::reconfigureCB(costmap_2d::VoxelPluginConfig &config, uint32_t level)
+{
+  enabled_ = config.enabled;
+  footprint_clearing_enabled_ = config.footprint_clearing_enabled;
+  max_obstacle_height_ = config.max_obstacle_height;
+  size_z_ = config.z_voxels;
+  origin_z_ = config.origin_z;
+  z_resolution_ = config.z_resolution;
+  unknown_threshold_ = config.unknown_threshold + (VOXEL_BITS - size_z_);
+  mark_threshold_ = config.mark_threshold;
+  combination_method_ = config.combination_method;
+  matchSize();
+}
+
+void VoxelLayer::matchSize()
+{
+  ObstacleLayer::matchSize();
+  voxel_grid_.resize(size_x_, size_y_, size_z_);
+  ROS_ASSERT(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_);
+}
+
+void VoxelLayer::reset()
+{
+  deactivate();
+  resetMaps();
+  voxel_grid_.reset();
+  activate();
+}
+
+void VoxelLayer::resetMaps()
+{
+  Costmap2D::resetMaps();
+  voxel_grid_.reset();
+}
+
+void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
+                                       double* min_y, double* max_x, double* max_y)
+{
+  if (rolling_window_)
+    updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
+  useExtraBounds(min_x, min_y, max_x, max_y);
+
+  bool current = true;
+  std::vector<Observation> observations, clearing_observations;
+
+  // get the marking observations
+  current = getMarkingObservations(observations) && current;
+
+  // get the clearing observations
+  current = getClearingObservations(clearing_observations) && current;
+
+  // update the global current status
+  current_ = current;
+
+  // raytrace freespace
+  for (unsigned int i = 0; i < clearing_observations.size(); ++i)
+  {
+    raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
+  }
+
+  // place the new obstacles into a priority queue... each with a priority of zero to begin with
+  for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
+  {
+    const Observation& obs = *it;
+
+    const sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
+
+    double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
+
+    sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
+    sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
+    sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
+
+    for (unsigned int i = 0; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
+    {
+      // if the obstacle is too high or too far away from the robot we won't add it
+      if (*iter_z > max_obstacle_height_)
+        continue;
+
+      // compute the squared distance from the hitpoint to the pointcloud's origin
+      double sq_dist = (*iter_x - obs.origin_.x) * (*iter_x - obs.origin_.x)
+                       + (*iter_y - obs.origin_.y) * (*iter_y - obs.origin_.y)
+                       + (*iter_z - obs.origin_.z) * (*iter_z - obs.origin_.z);
+
+      // if the point is far enough away... we won't consider it
+      if (sq_dist >= sq_obstacle_range)
+        continue;
+
+      // now we need to compute the map coordinates for the observation
+      unsigned int mx, my, mz;
+      if (*iter_z < origin_z_)
+      {
+        if (!worldToMap3D(*iter_x, *iter_y, origin_z_, mx, my, mz))
+          continue;
+      }
+      else if (!worldToMap3D(*iter_x, *iter_y, *iter_z, mx, my, mz))
+      {
+        continue;
+      }
+
+      // mark the cell in the voxel grid and check if we should also mark it in the costmap
+      if (voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_))
+      {
+        unsigned int index = getIndex(mx, my);
+
+        costmap_[index] = LETHAL_OBSTACLE;
+        touch(double(*iter_x), double(*iter_y), min_x, min_y, max_x, max_y);
+      }
+    }
+  }
+
+  if (publish_voxel_)
+  {
+    costmap_2d::VoxelGrid grid_msg;
+    unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY();
+    grid_msg.size_x = voxel_grid_.sizeX();
+    grid_msg.size_y = voxel_grid_.sizeY();
+    grid_msg.size_z = voxel_grid_.sizeZ();
+    grid_msg.data.resize(size);
+    memcpy(&grid_msg.data[0], voxel_grid_.getData(), size * sizeof(unsigned int));
+
+    grid_msg.origin.x = origin_x_;
+    grid_msg.origin.y = origin_y_;
+    grid_msg.origin.z = origin_z_;
+
+    grid_msg.resolutions.x = resolution_;
+    grid_msg.resolutions.y = resolution_;
+    grid_msg.resolutions.z = z_resolution_;
+    grid_msg.header.frame_id = global_frame_;
+    grid_msg.header.stamp = ros::Time::now();
+    voxel_pub_.publish(grid_msg);
+  }
+
+  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
+}
+
+void VoxelLayer::clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info)
+{
+  // get the cell coordinates of the center point of the window
+  unsigned int mx, my;
+  if (!worldToMap(wx, wy, mx, my))
+    return;
+
+  // compute the bounds of the window
+  double start_x = wx - w_size_x / 2;
+  double start_y = wy - w_size_y / 2;
+  double end_x = start_x + w_size_x;
+  double end_y = start_y + w_size_y;
+
+  // scale the window based on the bounds of the costmap
+  start_x = std::max(origin_x_, start_x);
+  start_y = std::max(origin_y_, start_y);
+
+  end_x = std::min(origin_x_ + getSizeInMetersX(), end_x);
+  end_y = std::min(origin_y_ + getSizeInMetersY(), end_y);
+
+  // get the map coordinates of the bounds of the window
+  unsigned int map_sx, map_sy, map_ex, map_ey;
+
+  // check for legality just in case
+  if (!worldToMap(start_x, start_y, map_sx, map_sy) || !worldToMap(end_x, end_y, map_ex, map_ey))
+    return;
+
+  // we know that we want to clear all non-lethal obstacles in this window to get it ready for inflation
+  unsigned int index = getIndex(map_sx, map_sy);
+  unsigned char* current = &costmap_[index];
+  for (unsigned int j = map_sy; j <= map_ey; ++j)
+  {
+    for (unsigned int i = map_sx; i <= map_ex; ++i)
+    {
+      // if the cell is a lethal obstacle... we'll keep it and queue it, otherwise... we'll clear it
+      if (*current != LETHAL_OBSTACLE)
+      {
+        if (clear_no_info || *current != NO_INFORMATION)
+        {
+          *current = FREE_SPACE;
+          voxel_grid_.clearVoxelColumn(index);
+        }
+      }
+      current++;
+      index++;
+    }
+    current += size_x_ - (map_ex - map_sx) - 1;
+    index += size_x_ - (map_ex - map_sx) - 1;
+  }
+}
+
+void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
+                                           double* max_x, double* max_y)
+{
+  size_t clearing_observation_cloud_size = clearing_observation.cloud_->height * clearing_observation.cloud_->width;
+  if (clearing_observation_cloud_size == 0)
+    return;
+
+  double sensor_x, sensor_y, sensor_z;
+  double ox = clearing_observation.origin_.x;
+  double oy = clearing_observation.origin_.y;
+  double oz = clearing_observation.origin_.z;
+
+  if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
+  {
+    ROS_WARN_THROTTLE(
+        1.0,
+        "The origin for the sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
+        ox, oy, oz);
+    return;
+  }
+
+  bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0);
+  if (publish_clearing_points)
+  {
+    clearing_endpoints_.points.clear();
+    clearing_endpoints_.points.reserve(clearing_observation_cloud_size);
+  }
+
+  // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
+  double map_end_x = origin_x_ + getSizeInMetersX();
+  double map_end_y = origin_y_ + getSizeInMetersY();
+
+  sensor_msgs::PointCloud2ConstIterator<float> iter_x(*(clearing_observation.cloud_), "x");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_y(*(clearing_observation.cloud_), "y");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_z(*(clearing_observation.cloud_), "z");
+
+  for (;iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
+  {
+    double wpx = *iter_x;
+    double wpy = *iter_y;
+    double wpz = *iter_z;
+
+    double distance = dist(ox, oy, oz, wpx, wpy, wpz);
+    double scaling_fact = 1.0;
+    scaling_fact = std::max(std::min(scaling_fact, (distance - 2 * resolution_) / distance), 0.0);
+    wpx = scaling_fact * (wpx - ox) + ox;
+    wpy = scaling_fact * (wpy - oy) + oy;
+    wpz = scaling_fact * (wpz - oz) + oz;
+
+    double a = wpx - ox;
+    double b = wpy - oy;
+    double c = wpz - oz;
+    double t = 1.0;
+
+    // we can only raytrace to a maximum z height
+    if (wpz > max_obstacle_height_)
+    {
+      // we know we want the vector's z value to be max_z
+      t = std::max(0.0, std::min(t, (max_obstacle_height_ - 0.01 - oz) / c));
+    }
+    // and we can only raytrace down to the floor
+    else if (wpz < origin_z_)
+    {
+      // we know we want the vector's z value to be 0.0
+      t = std::min(t, (origin_z_ - oz) / c);
+    }
+
+    // the minimum value to raytrace from is the origin
+    if (wpx < origin_x_)
+    {
+      t = std::min(t, (origin_x_ - ox) / a);
+    }
+    if (wpy < origin_y_)
+    {
+      t = std::min(t, (origin_y_ - oy) / b);
+    }
+
+    // the maximum value to raytrace to is the end of the map
+    if (wpx > map_end_x)
+    {
+      t = std::min(t, (map_end_x - ox) / a);
+    }
+    if (wpy > map_end_y)
+    {
+      t = std::min(t, (map_end_y - oy) / b);
+    }
+
+    wpx = ox + a * t;
+    wpy = oy + b * t;
+    wpz = oz + c * t;
+
+    double point_x, point_y, point_z;
+    if (worldToMap3DFloat(wpx, wpy, wpz, point_x, point_y, point_z))
+    {
+      unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
+
+      // voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
+      voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,
+                                      unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
+                                      cell_raytrace_range);
+
+      updateRaytraceBounds(ox, oy, wpx, wpy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
+
+      if (publish_clearing_points)
+      {
+        geometry_msgs::Point32 point;
+        point.x = wpx;
+        point.y = wpy;
+        point.z = wpz;
+        clearing_endpoints_.points.push_back(point);
+      }
+    }
+  }
+
+  if (publish_clearing_points)
+  {
+    clearing_endpoints_.header.frame_id = global_frame_;
+    clearing_endpoints_.header.stamp = clearing_observation.cloud_->header.stamp;
+    clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq;
+
+    clearing_endpoints_pub_.publish(clearing_endpoints_);
+  }
+}
+
+void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
+{
+  // project the new origin into the grid
+  int cell_ox, cell_oy;
+  cell_ox = int((new_origin_x - origin_x_) / resolution_);
+  cell_oy = int((new_origin_y - origin_y_) / resolution_);
+
+  // compute the associated world coordinates for the origin cell
+  // beacuase we want to keep things grid-aligned
+  double new_grid_ox, new_grid_oy;
+  new_grid_ox = origin_x_ + cell_ox * resolution_;
+  new_grid_oy = origin_y_ + cell_oy * resolution_;
+
+  // To save casting from unsigned int to int a bunch of times
+  int size_x = size_x_;
+  int size_y = size_y_;
+
+  // we need to compute the overlap of the new and existing windows
+  int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
+  lower_left_x = std::min(std::max(cell_ox, 0), size_x);
+  lower_left_y = std::min(std::max(cell_oy, 0), size_y);
+  upper_right_x = std::min(std::max(cell_ox + size_x, 0), size_x);
+  upper_right_y = std::min(std::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;
+
+  // we need a map to store the obstacles in the window temporarily
+  unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
+  unsigned int* local_voxel_map = new unsigned int[cell_size_x * cell_size_y];
+  unsigned int* voxel_map = voxel_grid_.getData();
+
+  // copy the local window in the costmap to the local map
+  copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
+  copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0, cell_size_x, cell_size_x,
+                cell_size_y);
+
+  // we'll reset our maps to unknown space if appropriate
+  resetMaps();
+
+  // update the origin with the appropriate world coordinates
+  origin_x_ = new_grid_ox;
+  origin_y_ = new_grid_oy;
+
+  // compute the starting cell location for copying data back in
+  int start_x = lower_left_x - cell_ox;
+  int start_y = lower_left_y - cell_oy;
+
+  // now we want to copy the overlapping information back into the map, but in its new location
+  copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
+  copyMapRegion(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_, cell_size_x, cell_size_y);
+
+  // make sure to clean up
+  delete[] local_map;
+  delete[] local_voxel_map;
+}
+
+}  // namespace costmap_2d