diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md
index aee685e6c88a74b6d5d53f4c12cf2d0fedd09952..f4a7089790c9c17da9af88d4959f085289f70622 100644
--- a/doc/parameters/param_list.md
+++ b/doc/parameters/param_list.md
@@ -51,11 +51,13 @@ For example:
```yaml
local_costmap:
ros__parameters:
- plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
+ plugins: ["obstacle_layer", "voxel_layer", "sonar_layer", "inflation_layer"]
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
voxel_layer:
plugin: "nav2_costmap_2d::VoxelLayer"
+ sonar_layer:
+ plugin: "nav2_costmap_2d::RangeSensorLayer"
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
```
@@ -115,7 +117,23 @@ When `plugins` parameter is not overridden, the following default plugins are lo
| `<data source>`.marking | true | Whether source should mark in costmap |
| `<data source>`.clearing | false | Whether source should raytrace clear in costmap |
| `<data source>`.obstacle_range | 2.5 | Maximum range to mark obstacles in costmap |
-| `<data source>`.raytrace_range | 3.0 | Maximum range to raytrace clear obstacles from costmap |
+| `<data source>`.raytrace_range | 3.0 | Maximum range to raytrace clear obstacles from costmap |
+
+## range_sensor_layer plugin
+
+* `<range layer>`: Name corresponding to the `nav2_costmap_2d::RangeSensorLayer` plugin. This name gets defined in `plugin_names`.
+
+| Parameter | Default | Description |
+| ----------| --------| ------------|
+| `<range layer>`.enabled | true | Whether it is enabled |
+| `<range layer>`.topics | [""] | Range topics to subscribe to |
+| `<range layer>`.phi | 1.2 | Phi value |
+| `<range layer>`.inflate_cone | 1.0 | Inflate the triangular area covered by the sensor (percentage) |
+| `<range layer>`.no_readings_timeout | 0.0 | No Readings Timeout |
+| `<range layer>`.clear_threshold | 0.2 | Probability below which cells are marked as free |
+| `<range layer>`.mark_threshold | 0.8 | Probability above which cells are marked as occupied |
+| `<range layer>`.clear_on_max_reading | false | Clear on max reading |
+| `<range layer>`.input_sensor_type | ALL | Input sensor type either ALL (automatic selection), VARIABLE (min range != max range), or FIXED (min range == max range) |
## voxel_layer plugin
diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt
index d7508415edff0898336a5dc532fa2d310d7278c7..4c9a2a93666e011cf4e4c222bb40c016ec9337c9 100644
--- a/nav2_costmap_2d/CMakeLists.txt
+++ b/nav2_costmap_2d/CMakeLists.txt
@@ -22,6 +22,7 @@ find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_sensor_msgs REQUIRED)
find_package(visualization_msgs REQUIRED)
+find_package(angles REQUIRED)
remove_definitions(-DDISABLE_LIBUSB-1.0)
find_package(Eigen3 REQUIRED)
@@ -72,6 +73,7 @@ set(dependencies
tf2_ros
tf2_sensor_msgs
visualization_msgs
+ angles
)
ament_target_dependencies(nav2_costmap_2d_core
@@ -84,6 +86,7 @@ add_library(layers SHARED
plugins/obstacle_layer.cpp
src/observation_buffer.cpp
plugins/voxel_layer.cpp
+ plugins/range_sensor_layer.cpp
)
ament_target_dependencies(layers
${dependencies}
diff --git a/nav2_costmap_2d/costmap_plugins.xml b/nav2_costmap_2d/costmap_plugins.xml
index 62bfac732b959e935f4c1d8390ad208be313c2bd..a072539233252737c872ef6450ff93178e59d078 100644
--- a/nav2_costmap_2d/costmap_plugins.xml
+++ b/nav2_costmap_2d/costmap_plugins.xml
@@ -12,6 +12,9 @@
<class type="nav2_costmap_2d::VoxelLayer" base_class_type="nav2_costmap_2d::Layer">
<description>Similar to obstacle costmap, but uses 3D voxel grid to store data.</description>
</class>
+ <class type="nav2_costmap_2d::RangeSensorLayer" base_class_type="nav2_costmap_2d::Layer">
+ <description>A range-sensor (sonar, IR) based obstacle layer for costmap_2d</description>
+ </class>
</library>
</class_libraries>
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..ce2861599d7a304d67c56e34bef0ec4261360695
--- /dev/null
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp
@@ -0,0 +1,138 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018 David V. Lu!!
+ * Copyright (c) 2020, Bytes Robotics
+ * 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 copyright holder 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 HOLDER 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.
+ */
+
+#ifndef NAV2_COSTMAP_2D__RANGE_SENSOR_LAYER_HPP_
+#define NAV2_COSTMAP_2D__RANGE_SENSOR_LAYER_HPP_
+
+#include <list>
+#include <string>
+#include <vector>
+#include <mutex>
+
+#include "map_msgs/msg/occupancy_grid_update.hpp"
+#include "message_filters/subscriber.h"
+#include "nav2_costmap_2d/costmap_layer.hpp"
+#include "nav2_costmap_2d/layered_costmap.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include "sensor_msgs/msg/range.hpp"
+
+namespace nav2_costmap_2d
+{
+
+class RangeSensorLayer : public CostmapLayer
+{
+public:
+ enum class InputSensorType
+ {
+ VARIABLE,
+ FIXED,
+ ALL
+ };
+
+ RangeSensorLayer();
+
+ virtual void onInitialize();
+ virtual void updateBounds(
+ double robot_x, double robot_y, double robot_yaw,
+ double * min_x, double * min_y, double * max_x, double * max_y);
+ virtual void updateCosts(
+ nav2_costmap_2d::Costmap2D & master_grid, int min_i,
+ int min_j, int max_i, int max_j);
+ virtual void reset();
+ virtual void deactivate();
+ virtual void activate();
+
+ void bufferIncomingRangeMsg(const sensor_msgs::msg::Range::SharedPtr range_message);
+
+private:
+ void updateCostmap();
+ void updateCostmap(sensor_msgs::msg::Range & range_message, bool clear_sensor_cone);
+
+ void processRangeMsg(sensor_msgs::msg::Range & range_message);
+ void processFixedRangeMsg(sensor_msgs::msg::Range & range_message);
+ void processVariableRangeMsg(sensor_msgs::msg::Range & range_message);
+
+ void resetRange();
+
+ inline double gamma(double theta);
+ inline double delta(double phi);
+ inline double sensor_model(double r, double phi, double theta);
+
+ inline void get_deltas(double angle, double * dx, double * dy);
+ inline void update_cell(
+ double ox, double oy, double ot,
+ double r, double nx, double ny, bool clear);
+
+ inline double to_prob(unsigned char c)
+ {
+ return static_cast<double>(c) / nav2_costmap_2d::LETHAL_OBSTACLE;
+ }
+ inline unsigned char to_cost(double p)
+ {
+ return static_cast<unsigned char>(p * nav2_costmap_2d::LETHAL_OBSTACLE);
+ }
+
+ std::function<void(sensor_msgs::msg::Range & range_message)> processRangeMessageFunc_;
+ std::mutex range_message_mutex_;
+ std::list<sensor_msgs::msg::Range> range_msgs_buffer_;
+
+ double max_angle_, phi_v_;
+ double inflate_cone_;
+ std::string global_frame_;
+
+ double clear_threshold_, mark_threshold_;
+ bool clear_on_max_reading_;
+
+ tf2::Duration transform_tolerance_;
+ double no_readings_timeout_;
+ rclcpp::Time last_reading_time_;
+ unsigned int buffered_readings_;
+ std::vector<rclcpp::Subscription<sensor_msgs::msg::Range>::SharedPtr> range_subs_;
+ double min_x_, min_y_, max_x_, max_y_;
+
+ float area(int x1, int y1, int x2, int y2, int x3, int y3)
+ {
+ return fabs((x1 * (y2 - y3) + x2 * (y3 - y1) + x3 * (y1 - y2)) / 2.0);
+ }
+
+ int orient2d(int Ax, int Ay, int Bx, int By, int Cx, int Cy)
+ {
+ return (Bx - Ax) * (Cy - Ay) - (By - Ay) * (Cx - Ax);
+ }
+};
+} // namespace nav2_costmap_2d
+#endif // NAV2_COSTMAP_2D__RANGE_SENSOR_LAYER_HPP_
diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp
index 86561d334b3b0290f3ead3e45dfb905470884f58..df8405a3c91ba8b2f72e2528fc6f806a97eee3c2 100644
--- a/nav2_costmap_2d/plugins/inflation_layer.cpp
+++ b/nav2_costmap_2d/plugins/inflation_layer.cpp
@@ -40,6 +40,8 @@
#include <limits>
#include <map>
#include <vector>
+#include <algorithm>
+#include <utility>
#include "nav2_costmap_2d/costmap_math.hpp"
#include "nav2_costmap_2d/footprint.hpp"
@@ -163,7 +165,7 @@ InflationLayer::updateCosts(
}
// make sure the inflation list is empty at the beginning of the cycle (should always be true)
- for (auto & dist:inflation_cells_) {
+ for (auto & dist : inflation_cells_) {
RCLCPP_FATAL_EXPRESSION(
rclcpp::get_logger("nav2_costmap_2d"),
!dist.empty(), "The inflation list must be empty at the beginning of inflation");
@@ -215,9 +217,10 @@ InflationLayer::updateCosts(
// Process cells by increasing distance; new cells are appended to the
// corresponding distance bin, so they
// can overtake previously inserted but farther away cells
- for (const auto & dist_bin: inflation_cells_) {
+ for (const auto & dist_bin : inflation_cells_) {
for (std::size_t i = 0; i < dist_bin.size(); ++i) {
- // Do not use iterator or for-range based loops to iterate though dist_bin, since it's size might
+ // Do not use iterator or for-range based loops to
+ // iterate though dist_bin, since it's size might
// change when a new cell is enqueued, invalidating all iterators
unsigned int index = dist_bin[i].index_;
@@ -260,7 +263,7 @@ InflationLayer::updateCosts(
}
}
- for (auto & dist:inflation_cells_) {
+ for (auto & dist : inflation_cells_) {
dist.clear();
dist.reserve(200);
}
diff --git a/nav2_costmap_2d/plugins/range_sensor_layer.cpp b/nav2_costmap_2d/plugins/range_sensor_layer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8a6124a47a9282098485f6238aa855de9e5ff3c2
--- /dev/null
+++ b/nav2_costmap_2d/plugins/range_sensor_layer.cpp
@@ -0,0 +1,524 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018 David V. Lu!!
+ * Copyright (c) 2020, Bytes Robotics
+ * 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 copyright holder 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 HOLDER 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 <angles/angles.h>
+#include <algorithm>
+#include <list>
+#include <limits>
+#include <string>
+#include <vector>
+
+#include "pluginlib/class_list_macros.hpp"
+#include "geometry_msgs/msg/point_stamped.hpp"
+#include "nav2_costmap_2d/range_sensor_layer.hpp"
+
+PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::RangeSensorLayer, nav2_costmap_2d::Layer)
+
+using nav2_costmap_2d::LETHAL_OBSTACLE;
+using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
+using nav2_costmap_2d::NO_INFORMATION;
+
+using namespace std::literals::chrono_literals;
+
+namespace nav2_costmap_2d
+{
+
+RangeSensorLayer::RangeSensorLayer() {}
+
+void RangeSensorLayer::onInitialize()
+{
+ current_ = true;
+ buffered_readings_ = 0;
+ last_reading_time_ = node_->now();
+ default_value_ = to_cost(0.5);
+
+ matchSize();
+ resetRange();
+
+ declareParameter("enabled", rclcpp::ParameterValue(true));
+ node_->get_parameter(name_ + "." + "enabled", enabled_);
+ declareParameter("phi", rclcpp::ParameterValue(1.2));
+ node_->get_parameter(name_ + "." + "phi", phi_v_);
+ declareParameter("inflate_cone", rclcpp::ParameterValue(1.0));
+ node_->get_parameter(name_ + "." + "phi", phi_v_);
+ declareParameter("no_readings_timeout", rclcpp::ParameterValue(0.0));
+ node_->get_parameter(name_ + "." + "no_readings_timeout", no_readings_timeout_);
+ declareParameter("clear_threshold", rclcpp::ParameterValue(0.2));
+ node_->get_parameter(name_ + "." + "clear_threshold", clear_threshold_);
+ declareParameter("mark_threshold", rclcpp::ParameterValue(0.8));
+ node_->get_parameter(name_ + "." + "mark_threshold", mark_threshold_);
+ declareParameter("clear_on_max_reading", rclcpp::ParameterValue(false));
+ node_->get_parameter(name_ + "." + "clear_on_max_reading", clear_on_max_reading_);
+
+ double temp_tf_tol = 0.0;
+ node_->get_parameter("transform_tolerance", temp_tf_tol);
+ transform_tolerance_ = tf2::durationFromSec(temp_tf_tol);
+
+ std::vector<std::string> topic_names{};
+ declareParameter("topics", rclcpp::ParameterValue(topic_names));
+ node_->get_parameter(name_ + "." + "topics", topic_names);
+
+ InputSensorType input_sensor_type = InputSensorType::ALL;
+ std::string sensor_type_name;
+ declareParameter("input_sensor_type", rclcpp::ParameterValue("ALL"));
+ node_->get_parameter(name_ + "." + "input_sensor_type", sensor_type_name);
+
+ std::transform(
+ sensor_type_name.begin(), sensor_type_name.end(),
+ sensor_type_name.begin(), ::toupper);
+ RCLCPP_INFO(
+ node_->get_logger(), "%s: %s as input_sensor_type given",
+ name_.c_str(), sensor_type_name.c_str());
+
+ if (sensor_type_name == "VARIABLE") {
+ input_sensor_type = InputSensorType::VARIABLE;
+ } else if (sensor_type_name == "FIXED") {
+ input_sensor_type = InputSensorType::FIXED;
+ } else if (sensor_type_name == "ALL") {
+ input_sensor_type = InputSensorType::ALL;
+ } else {
+ RCLCPP_ERROR(
+ node_->get_logger(), "%s: Invalid input sensor type: %s. Defaulting to ALL.",
+ name_.c_str(), sensor_type_name.c_str());
+ }
+
+ // Validate topic names list: it must be a (normally non-empty) list of strings
+ if (topic_names.empty()) {
+ RCLCPP_FATAL(
+ node_->get_logger(), "Invalid topic names list: it must"
+ "be a non-empty list of strings");
+ return;
+ }
+
+ // Traverse the topic names list subscribing to all of them with the same callback method
+ for (auto & topic_name : topic_names) {
+ if (input_sensor_type == InputSensorType::VARIABLE) {
+ processRangeMessageFunc_ = std::bind(
+ &RangeSensorLayer::processVariableRangeMsg, this,
+ std::placeholders::_1);
+ } else if (input_sensor_type == InputSensorType::FIXED) {
+ processRangeMessageFunc_ = std::bind(
+ &RangeSensorLayer::processFixedRangeMsg, this,
+ std::placeholders::_1);
+ } else if (input_sensor_type == InputSensorType::ALL) {
+ processRangeMessageFunc_ = std::bind(
+ &RangeSensorLayer::processRangeMsg, this,
+ std::placeholders::_1);
+ } else {
+ RCLCPP_ERROR(
+ node_->get_logger(),
+ "%s: Invalid input sensor type: %s. Did you make a new type"
+ "and forgot to choose the subscriber for it?",
+ name_.c_str(), sensor_type_name.c_str());
+ }
+ range_subs_.push_back(
+ node_->create_subscription<sensor_msgs::msg::Range>(
+ topic_name, rclcpp::SensorDataQoS(), std::bind(
+ &RangeSensorLayer::bufferIncomingRangeMsg, this,
+ std::placeholders::_1)));
+
+ RCLCPP_INFO(
+ node_->get_logger(), "RangeSensorLayer: subscribed to "
+ "topic %s", range_subs_.back()->get_topic_name());
+ }
+ global_frame_ = layered_costmap_->getGlobalFrameID();
+}
+
+
+double RangeSensorLayer::gamma(double theta)
+{
+ if (fabs(theta) > max_angle_) {
+ return 0.0;
+ } else {
+ return 1 - pow(theta / max_angle_, 2);
+ }
+}
+
+double RangeSensorLayer::delta(double phi)
+{
+ return 1 - (1 + tanh(2 * (phi - phi_v_))) / 2;
+}
+
+void RangeSensorLayer::get_deltas(double angle, double * dx, double * dy)
+{
+ double ta = tan(angle);
+ if (ta == 0) {
+ *dx = 0;
+ } else {
+ *dx = resolution_ / ta;
+ }
+
+ *dx = copysign(*dx, cos(angle));
+ *dy = copysign(resolution_, sin(angle));
+}
+
+double RangeSensorLayer::sensor_model(double r, double phi, double theta)
+{
+ double lbda = delta(phi) * gamma(theta);
+
+ double delta = resolution_;
+
+ if (phi >= 0.0 && phi < r - 2 * delta * r) {
+ return (1 - lbda) * (0.5);
+ } else if (phi < r - delta * r) {
+ return lbda * 0.5 * pow((phi - (r - 2 * delta * r)) / (delta * r), 2) +
+ (1 - lbda) * .5;
+ } else if (phi < r + delta * r) {
+ double J = (r - phi) / (delta * r);
+ return lbda * ((1 - (0.5) * pow(J, 2)) - 0.5) + 0.5;
+ } else {
+ return 0.5;
+ }
+}
+
+void RangeSensorLayer::bufferIncomingRangeMsg(
+ const sensor_msgs::msg::Range::SharedPtr range_message)
+{
+ range_message_mutex_.lock();
+ range_msgs_buffer_.push_back(*range_message);
+ range_message_mutex_.unlock();
+}
+
+void RangeSensorLayer::updateCostmap()
+{
+ std::list<sensor_msgs::msg::Range> range_msgs_buffer_copy;
+
+ range_message_mutex_.lock();
+ range_msgs_buffer_copy = std::list<sensor_msgs::msg::Range>(range_msgs_buffer_);
+ range_msgs_buffer_.clear();
+ range_message_mutex_.unlock();
+
+ for (auto & range_msgs_it : range_msgs_buffer_copy) {
+ processRangeMessageFunc_(range_msgs_it);
+ }
+}
+
+void RangeSensorLayer::processRangeMsg(sensor_msgs::msg::Range & range_message)
+{
+ if (range_message.min_range == range_message.max_range) {
+ processFixedRangeMsg(range_message);
+ } else {
+ processVariableRangeMsg(range_message);
+ }
+}
+
+void RangeSensorLayer::processFixedRangeMsg(sensor_msgs::msg::Range & range_message)
+{
+ if (!std::isinf(range_message.range)) {
+ RCLCPP_ERROR(
+ node_->get_logger(),
+ "Fixed distance ranger (min_range == max_range) in frame %s sent invalid value. "
+ "Only -Inf (== object detected) and Inf (== no object detected) are valid.",
+ range_message.header.frame_id.c_str());
+ return;
+ }
+
+ bool clear_sensor_cone = false;
+
+ if (range_message.range > 0) { // +inf
+ if (!clear_on_max_reading_) {
+ return; // no clearing at all
+ }
+ clear_sensor_cone = true;
+ }
+
+ range_message.range = range_message.min_range;
+
+ updateCostmap(range_message, clear_sensor_cone);
+}
+
+void RangeSensorLayer::processVariableRangeMsg(sensor_msgs::msg::Range & range_message)
+{
+ if (range_message.range < range_message.min_range || range_message.range >
+ range_message.max_range)
+ {
+ return;
+ }
+
+ bool clear_sensor_cone = false;
+
+ if (range_message.range >= range_message.max_range && clear_on_max_reading_) {
+ clear_sensor_cone = true;
+ }
+
+ updateCostmap(range_message, clear_sensor_cone);
+}
+
+void RangeSensorLayer::updateCostmap(
+ sensor_msgs::msg::Range & range_message,
+ bool clear_sensor_cone)
+{
+ max_angle_ = range_message.field_of_view / 2;
+
+ geometry_msgs::msg::PointStamped in, out;
+ in.header.stamp = range_message.header.stamp;
+ in.header.frame_id = range_message.header.frame_id;
+
+ if (!tf_->canTransform(
+ in.header.frame_id, global_frame_, tf2_ros::fromMsg(in.header.stamp)))
+ {
+ RCLCPP_INFO(
+ node_->get_logger(), "Range sensor layer can't transform from %s to %s",
+ global_frame_.c_str(), in.header.frame_id.c_str());
+ return;
+ }
+
+ tf_->transform(in, out, global_frame_, transform_tolerance_);
+
+ double ox = out.point.x, oy = out.point.y;
+
+ in.point.x = range_message.range;
+
+ tf_->transform(in, out, global_frame_, transform_tolerance_);
+
+ double tx = out.point.x, ty = out.point.y;
+
+ // calculate target props
+ double dx = tx - ox, dy = ty - oy, theta = atan2(dy, dx), d = sqrt(dx * dx + dy * dy);
+
+ // Integer Bounds of Update
+ int bx0, by0, bx1, by1;
+
+ // Triangle that will be really updated; the other cells within bounds are ignored
+ // This triangle is formed by the origin and left and right sides of sonar cone
+ int Ox, Oy, Ax, Ay, Bx, By;
+
+ // Bounds includes the origin
+ worldToMapNoBounds(ox, oy, Ox, Oy);
+ bx1 = bx0 = Ox;
+ by1 = by0 = Oy;
+ touch(ox, oy, &min_x_, &min_y_, &max_x_, &max_y_);
+
+ // Update Map with Target Point
+ unsigned int aa, ab;
+ if (worldToMap(tx, ty, aa, ab)) {
+ setCost(aa, ab, 233);
+ touch(tx, ty, &min_x_, &min_y_, &max_x_, &max_y_);
+ }
+
+ double mx, my;
+
+ // Update left side of sonar cone
+ mx = ox + cos(theta - max_angle_) * d * 1.2;
+ my = oy + sin(theta - max_angle_) * d * 1.2;
+ worldToMapNoBounds(mx, my, Ax, Ay);
+ bx0 = std::min(bx0, Ax);
+ bx1 = std::max(bx1, Ax);
+ by0 = std::min(by0, Ay);
+ by1 = std::max(by1, Ay);
+ touch(mx, my, &min_x_, &min_y_, &max_x_, &max_y_);
+
+ // Update right side of sonar cone
+ mx = ox + cos(theta + max_angle_) * d * 1.2;
+ my = oy + sin(theta + max_angle_) * d * 1.2;
+
+ worldToMapNoBounds(mx, my, Bx, By);
+ bx0 = std::min(bx0, Bx);
+ bx1 = std::max(bx1, Bx);
+ by0 = std::min(by0, By);
+ by1 = std::max(by1, By);
+ touch(mx, my, &min_x_, &min_y_, &max_x_, &max_y_);
+
+ // Limit Bounds to Grid
+ bx0 = std::max(0, bx0);
+ by0 = std::max(0, by0);
+ bx1 = std::min(static_cast<int>(size_x_), bx1);
+ by1 = std::min(static_cast<int>(size_y_), by1);
+
+ for (unsigned int x = bx0; x <= (unsigned int)bx1; x++) {
+ for (unsigned int y = by0; y <= (unsigned int)by1; y++) {
+ bool update_xy_cell = true;
+
+ // Unless inflate_cone_ is set to 100 %, we update cells only within the
+ // (partially inflated) sensor cone, projected on the costmap as a triangle.
+ // 0 % corresponds to just the triangle, but if your sensor fov is very
+ // narrow, the covered area can become zero due to cell discretization.
+ // See wiki description for more details
+ if (inflate_cone_ < 1.0) {
+ // Determine barycentric coordinates
+ int w0 = orient2d(Ax, Ay, Bx, By, x, y);
+ int w1 = orient2d(Bx, By, Ox, Oy, x, y);
+ int w2 = orient2d(Ox, Oy, Ax, Ay, x, y);
+
+ // Barycentric coordinates inside area threshold; this is not mathematically
+ // sound at all, but it works!
+ float bcciath = -static_cast<float>(inflate_cone_) * area(Ax, Ay, Bx, By, Ox, Oy);
+ update_xy_cell = w0 >= bcciath && w1 >= bcciath && w2 >= bcciath;
+ }
+
+ if (update_xy_cell) {
+ double wx, wy;
+ mapToWorld(x, y, wx, wy);
+ update_cell(ox, oy, theta, range_message.range, wx, wy, clear_sensor_cone);
+ }
+ }
+ }
+
+ buffered_readings_++;
+ last_reading_time_ = node_->now();
+}
+
+void RangeSensorLayer::update_cell(
+ double ox, double oy, double ot, double r,
+ double nx, double ny, bool clear)
+{
+ unsigned int x, y;
+ if (worldToMap(nx, ny, x, y)) {
+ double dx = nx - ox, dy = ny - oy;
+ double theta = atan2(dy, dx) - ot;
+ theta = angles::normalize_angle(theta);
+ double phi = sqrt(dx * dx + dy * dy);
+ double sensor = 0.0;
+ if (!clear) {
+ sensor = sensor_model(r, phi, theta);
+ }
+ double prior = to_prob(getCost(x, y));
+ double prob_occ = sensor * prior;
+ double prob_not = (1 - sensor) * (1 - prior);
+ double new_prob = prob_occ / (prob_occ + prob_not);
+
+ RCLCPP_DEBUG(node_->get_logger(), "%f %f | %f %f = %f", dx, dy, theta, phi, sensor);
+ RCLCPP_DEBUG(node_->get_logger(), "%f | %f %f | %f", prior, prob_occ, prob_not, new_prob);
+ unsigned char c = to_cost(new_prob);
+ setCost(x, y, c);
+ }
+}
+
+void RangeSensorLayer::resetRange()
+{
+ min_x_ = min_y_ = std::numeric_limits<double>::max();
+ max_x_ = max_y_ = -std::numeric_limits<double>::max();
+}
+
+void RangeSensorLayer::updateBounds(
+ double robot_x, double robot_y,
+ double robot_yaw, double * min_x, double * min_y,
+ double * max_x, double * max_y)
+{
+ robot_yaw = 0 + robot_yaw; // Avoid error if variable not in use
+ if (layered_costmap_->isRolling()) {
+ updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
+ }
+
+ updateCostmap();
+
+ *min_x = std::min(*min_x, min_x_);
+ *min_y = std::min(*min_y, min_y_);
+ *max_x = std::max(*max_x, max_x_);
+ *max_y = std::max(*max_y, max_y_);
+
+ resetRange();
+
+ if (!enabled_) {
+ current_ = true;
+ return;
+ }
+
+ if (buffered_readings_ == 0) {
+ if (no_readings_timeout_ > 0.0 &&
+ (node_->now() - last_reading_time_).seconds() > no_readings_timeout_)
+ {
+ RCLCPP_WARN(
+ node_->get_logger(), "No range readings received for %.2f seconds, "
+ "while expected at least every %.2f seconds.",
+ (node_->now() - last_reading_time_).seconds(),
+ no_readings_timeout_);
+ current_ = false;
+ }
+ }
+}
+
+void RangeSensorLayer::updateCosts(
+ nav2_costmap_2d::Costmap2D & master_grid,
+ int min_i, int min_j, int max_i, int max_j)
+{
+ if (!enabled_) {
+ return;
+ }
+
+ unsigned char * master_array = master_grid.getCharMap();
+ unsigned int span = master_grid.getSizeInCellsX();
+ unsigned char clear = to_cost(clear_threshold_), mark = to_cost(mark_threshold_);
+
+ for (int j = min_j; j < max_j; j++) {
+ unsigned int it = j * span + min_i;
+ for (int i = min_i; i < max_i; i++) {
+ unsigned char prob = costmap_[it];
+ unsigned char current;
+ if (prob == nav2_costmap_2d::NO_INFORMATION) {
+ it++;
+ continue;
+ } else if (prob > mark) {
+ current = nav2_costmap_2d::LETHAL_OBSTACLE;
+ } else if (prob < clear) {
+ current = nav2_costmap_2d::FREE_SPACE;
+ } else {
+ it++;
+ continue;
+ }
+
+ unsigned char old_cost = master_array[it];
+
+ if (old_cost == NO_INFORMATION || old_cost < current) {
+ master_array[it] = current;
+ }
+ it++;
+ }
+ }
+
+ buffered_readings_ = 0;
+ current_ = true;
+}
+
+void RangeSensorLayer::reset()
+{
+ RCLCPP_DEBUG(node_->get_logger(), "Reseting range sensor layer...");
+ deactivate();
+ resetMaps();
+ current_ = true;
+ activate();
+}
+
+void RangeSensorLayer::deactivate()
+{
+ range_msgs_buffer_.clear();
+}
+
+void RangeSensorLayer::activate()
+{
+ range_msgs_buffer_.clear();
+}
+
+} // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/src/layered_costmap.cpp b/nav2_costmap_2d/src/layered_costmap.cpp
index 48ba1ea467db00bfa8d887cfdac1aec699eae3b5..a03b79c0dcde4c08c323a647f2aeea2cb58f595d 100644
--- a/nav2_costmap_2d/src/layered_costmap.cpp
+++ b/nav2_costmap_2d/src/layered_costmap.cpp
@@ -42,6 +42,7 @@
#include <memory>
#include <string>
#include <vector>
+#include <limits>
#include "nav2_costmap_2d/footprint.hpp"
diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt
index 4170ad3875b80bb246c6c8f30d9a2dc63cdd7700..5ecbeba923d69f06c2fe42af38342efe9c3c039a 100644
--- a/nav2_costmap_2d/test/integration/CMakeLists.txt
+++ b/nav2_costmap_2d/test/integration/CMakeLists.txt
@@ -43,6 +43,17 @@ target_link_libraries(obstacle_tests_exec
layers
)
+ament_add_gtest_executable(range_tests_exec
+ range_tests.cpp
+)
+ament_target_dependencies(range_tests_exec
+ ${dependencies}
+)
+target_link_libraries(range_tests_exec
+ nav2_costmap_2d_core
+ layers
+)
+
ament_add_test(test_collision_checker
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py"
@@ -83,6 +94,16 @@ ament_add_test(obstacle_tests
TEST_EXECUTABLE=$<TARGET_FILE:obstacle_tests_exec>
)
+ament_add_test(range_tests
+ GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+ COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py"
+ WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+ ENV
+ TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml
+ TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR}
+ TEST_EXECUTABLE=$<TARGET_FILE:range_tests_exec>
+)
+
## TODO(bpwilcox): this test (I believe) is intended to be launched with the simple_driving_test.xml,
## which has a dependency on rosbag playback
# ament_add_gtest_executable(costmap_tester
diff --git a/nav2_costmap_2d/test/integration/range_tests.cpp b/nav2_costmap_2d/test/integration/range_tests.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..196406e7e023bf2460816289eb32d162f19dbb0c
--- /dev/null
+++ b/nav2_costmap_2d/test/integration/range_tests.cpp
@@ -0,0 +1,293 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2020, Bytes Robotics
+ * 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 copyright holder 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 HOLDER 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 <memory>
+#include <string>
+#include <algorithm>
+#include <utility>
+#include <vector>
+
+#include "gtest/gtest.h"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/layered_costmap.hpp"
+#include "nav2_costmap_2d/observation_buffer.hpp"
+#include "../testing_helper.hpp"
+#include "sensor_msgs/msg/range.hpp"
+
+using std::begin;
+using std::end;
+using std::for_each;
+using std::all_of;
+using std::none_of;
+using std::pair;
+using std::string;
+
+class RclCppFixture
+{
+public:
+ RclCppFixture()
+ {
+ rclcpp::init(0, nullptr);
+ }
+
+ ~RclCppFixture()
+ {
+ rclcpp::shutdown();
+ }
+};
+
+RclCppFixture g_rclcppfixture;
+
+class TestLifecycleNode : public nav2_util::LifecycleNode
+{
+public:
+ explicit TestLifecycleNode(const string & name)
+ : nav2_util::LifecycleNode(name)
+ {
+ }
+
+ nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &)
+ {
+ return nav2_util::CallbackReturn::SUCCESS;
+ }
+
+ nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &)
+ {
+ return nav2_util::CallbackReturn::SUCCESS;
+ }
+
+ nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &)
+ {
+ return nav2_util::CallbackReturn::SUCCESS;
+ }
+
+ nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &)
+ {
+ return nav2_util::CallbackReturn::SUCCESS;
+ }
+
+ nav2_util::CallbackReturn onShutdown(const rclcpp_lifecycle::State &)
+ {
+ return nav2_util::CallbackReturn::SUCCESS;
+ }
+
+ nav2_util::CallbackReturn onError(const rclcpp_lifecycle::State &)
+ {
+ return nav2_util::CallbackReturn::SUCCESS;
+ }
+};
+
+class TestNode : public ::testing::Test
+{
+public:
+ TestNode()
+ : node_(std::make_shared<TestLifecycleNode>("range_test_node")),
+ tf_(node_->get_clock())
+ {
+ // Standard non-plugin specific parameters
+ node_->declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map")));
+ node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(false));
+ node_->declare_parameter("use_maximum", rclcpp::ParameterValue(false));
+ node_->declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100));
+ node_->declare_parameter(
+ "unknown_cost_value",
+ rclcpp::ParameterValue(static_cast<unsigned char>(0xff)));
+ node_->declare_parameter("trinary_costmap", rclcpp::ParameterValue(true));
+ node_->declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.3));
+ node_->declare_parameter("observation_sources", rclcpp::ParameterValue(std::string("range")));
+ node_->declare_parameter("global_frame", rclcpp::ParameterValue(std::string("map")));
+
+
+ // Range sensor specific parameters
+ node_->declare_parameter(
+ "range.topics",
+ rclcpp::ParameterValue(
+ std::vector<std::string>{"/range/topic"}));
+ node_->declare_parameter("range.phi", rclcpp::ParameterValue(1.2));
+ node_->declare_parameter("range.clear_on_max_reading", rclcpp::ParameterValue(true));
+ }
+
+ ~TestNode() {}
+
+protected:
+ std::shared_ptr<TestLifecycleNode> node_;
+ tf2_ros::Buffer tf_;
+};
+
+// Test clearing at max range
+TEST_F(TestNode, testClearingAtMaxRange) {
+ geometry_msgs::msg::TransformStamped transform;
+ transform.header.stamp = node_->now();
+ transform.header.frame_id = "frame";
+ transform.child_frame_id = "base_link";
+ transform.transform.translation.y = 5;
+ transform.transform.translation.x = 2;
+ tf_.setTransform(transform, "default_authority", true);
+
+ nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
+ layers.resizeMap(10, 10, 1, 0, 0);
+
+ std::shared_ptr<nav2_costmap_2d::RangeSensorLayer> rlayer{nullptr};
+ addRangeLayer(layers, tf_, node_, rlayer);
+
+ sensor_msgs::msg::Range msg;
+ msg.min_range = 1.0;
+ msg.max_range = 7.0;
+ msg.range = 2.0;
+ msg.header.stamp = node_->now();
+ msg.header.frame_id = "base_link";
+ msg.radiation_type = msg.ULTRASOUND;
+ msg.field_of_view = 0.174533; // 10 deg
+ rlayer->bufferIncomingRangeMsg(std::make_shared<sensor_msgs::msg::Range>(msg));
+
+ layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose
+// printMap(*(layers.getCostmap()));
+
+ ASSERT_EQ(layers.getCostmap()->getCost(4, 5), 254);
+
+ msg.range = 7.0;
+ msg.header.stamp = node_->now();
+ rlayer->bufferIncomingRangeMsg(std::make_shared<sensor_msgs::msg::Range>(msg));
+ layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose
+// printMap(*(layers.getCostmap()));
+
+ ASSERT_EQ(layers.getCostmap()->getCost(4, 5), 0);
+}
+
+// Testing fixed scan with robot forward motion
+TEST_F(TestNode, testProbabalisticModelForward) {
+ geometry_msgs::msg::TransformStamped transform;
+ transform.header.stamp = node_->now();
+ transform.header.frame_id = "frame";
+ transform.child_frame_id = "base_link";
+ transform.transform.translation.y = 5;
+ transform.transform.translation.x = 2;
+ tf_.setTransform(transform, "default_authority", true);
+
+ nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
+ layers.resizeMap(10, 10, 1, 0, 0);
+
+ std::shared_ptr<nav2_costmap_2d::RangeSensorLayer> rlayer{nullptr};
+ addRangeLayer(layers, tf_, node_, rlayer);
+
+ sensor_msgs::msg::Range msg;
+ msg.min_range = 1.0;
+ msg.max_range = 10.0;
+ msg.range = 3.0;
+ msg.header.stamp = node_->now();
+ msg.header.frame_id = "base_link";
+ msg.radiation_type = msg.ULTRASOUND;
+ msg.field_of_view = 0.174533; // 10 deg
+ rlayer->bufferIncomingRangeMsg(std::make_shared<sensor_msgs::msg::Range>(msg));
+
+ layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose
+// printMap(*(layers.getCostmap()));
+
+ rlayer->bufferIncomingRangeMsg(std::make_shared<sensor_msgs::msg::Range>(msg));
+ transform.transform.translation.y = 5;
+ transform.transform.translation.x = 4;
+ tf_.setTransform(transform, "default_authority", true);
+
+ layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose
+// printMap(*(layers.getCostmap()));
+
+ rlayer->bufferIncomingRangeMsg(std::make_shared<sensor_msgs::msg::Range>(msg));
+
+ transform.transform.translation.y = 5;
+ transform.transform.translation.x = 6;
+ tf_.setTransform(transform, "default_authority", true);
+
+ layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose
+// printMap(*(layers.getCostmap()));
+
+ ASSERT_EQ(layers.getCostmap()->getCost(5, 5), 254);
+ ASSERT_EQ(layers.getCostmap()->getCost(6, 5), 0);
+ ASSERT_EQ(layers.getCostmap()->getCost(7, 5), 254);
+ ASSERT_EQ(layers.getCostmap()->getCost(8, 5), 0);
+ ASSERT_EQ(layers.getCostmap()->getCost(9, 5), 254);
+}
+
+// Testing fixed motion with downward movement
+TEST_F(TestNode, testProbabalisticModelDownward) {
+ geometry_msgs::msg::TransformStamped transform;
+ transform.header.stamp = node_->now();
+ transform.header.frame_id = "frame";
+ transform.child_frame_id = "base_link";
+ transform.transform.translation.y = 3;
+ transform.transform.translation.x = 2;
+ tf_.setTransform(transform, "default_authority", true);
+
+ nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
+ layers.resizeMap(10, 10, 1, 0, 0);
+
+ std::shared_ptr<nav2_costmap_2d::RangeSensorLayer> rlayer{nullptr};
+ addRangeLayer(layers, tf_, node_, rlayer);
+
+ sensor_msgs::msg::Range msg;
+ msg.min_range = 1.0;
+ msg.max_range = 10.0;
+ msg.range = 1.0;
+ msg.header.stamp = node_->now();
+ msg.header.frame_id = "base_link";
+ msg.radiation_type = msg.ULTRASOUND;
+ msg.field_of_view = 0.174533; // 10 deg
+ rlayer->bufferIncomingRangeMsg(std::make_shared<sensor_msgs::msg::Range>(msg));
+
+ layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose
+// printMap(*(layers.getCostmap()));
+
+ rlayer->bufferIncomingRangeMsg(std::make_shared<sensor_msgs::msg::Range>(msg));
+
+ transform.transform.translation.y = 5;
+ transform.transform.translation.x = 2;
+ tf_.setTransform(transform, "default_authority", true);
+
+ layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose
+// printMap(*(layers.getCostmap()));
+
+ rlayer->bufferIncomingRangeMsg(std::make_shared<sensor_msgs::msg::Range>(msg));
+
+ transform.transform.translation.y = 7;
+ transform.transform.translation.x = 2;
+ tf_.setTransform(transform, "default_authority", true);
+
+ layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose
+// printMap(*(layers.getCostmap()));
+
+ ASSERT_EQ(layers.getCostmap()->getCost(3, 3), 254);
+ ASSERT_EQ(layers.getCostmap()->getCost(3, 4), 0);
+ ASSERT_EQ(layers.getCostmap()->getCost(3, 5), 254);
+ ASSERT_EQ(layers.getCostmap()->getCost(3, 6), 0);
+ ASSERT_EQ(layers.getCostmap()->getCost(3, 7), 254);
+}
diff --git a/nav2_costmap_2d/test/testing_helper.hpp b/nav2_costmap_2d/test/testing_helper.hpp
index 013341adf9071ebee4cc019affca8c2a4ad112e3..82c7d53ec2bc5ad66739c119052b1bb0ca0bdf4f 100644
--- a/nav2_costmap_2d/test/testing_helper.hpp
+++ b/nav2_costmap_2d/test/testing_helper.hpp
@@ -23,6 +23,7 @@
#include "nav2_costmap_2d/cost_values.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/static_layer.hpp"
+#include "nav2_costmap_2d/range_sensor_layer.hpp"
#include "nav2_costmap_2d/obstacle_layer.hpp"
#include "nav2_costmap_2d/inflation_layer.hpp"
#include "nav2_util/lifecycle_node.hpp"
@@ -97,6 +98,16 @@ void addObstacleLayer(
layers.addPlugin(std::shared_ptr<nav2_costmap_2d::Layer>(olayer));
}
+void addRangeLayer(
+ nav2_costmap_2d::LayeredCostmap & layers,
+ tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node,
+ std::shared_ptr<nav2_costmap_2d::RangeSensorLayer> & rlayer)
+{
+ rlayer = std::make_shared<nav2_costmap_2d::RangeSensorLayer>();
+ rlayer->initialize(&layers, "range", &tf, node, nullptr, nullptr /*TODO*/);
+ layers.addPlugin(std::shared_ptr<nav2_costmap_2d::Layer>(rlayer));
+}
+
void addObservation(
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer, double x, double y, double z = 0.0,
double ox = 0.0, double oy = 0.0, double oz = MAX_Z, bool marking = true, bool clearing = true)