Skip to content

Commit

Permalink
Add in range sensor costmap layer (ros-navigation#1888)
Browse files Browse the repository at this point in the history
* range costmap building

* range sensor layer working

* nav2 costmap pass linter and uncrustify tests

* Added back semicolon to range class

* Added docs

* Added angles dependency

* Added BSD license

* Added BSD license

* Made functions inline

* revmoed get_clock

* added input_sensor_type to docs

* Made defualt topic name empty to cause error

* using chorno literal to denote time units

* Added small initial test

* Fixed linter error, line breaks, enum, logger level, and transform_tolerance issue

* fixed segmentation fault in test and added transfrom tolerances to tf_->transform

* Got test to pass

* Added more tests

* removed incorrect parameter declaration

* Improved marking while moving tests and added clearing tests

* removed general clearing test

* changed testing helper import in test
  • Loading branch information
Michael-Equi authored and ruffsl committed Jul 2, 2021
1 parent e9f28f5 commit a3e0c2d
Show file tree
Hide file tree
Showing 10 changed files with 1,021 additions and 6 deletions.
22 changes: 20 additions & 2 deletions doc/parameters/param_list.md
Original file line number Diff line number Diff line change
Expand Up @@ -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"
```
Expand Down Expand Up @@ -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

Expand Down
3 changes: 3 additions & 0 deletions nav2_costmap_2d/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -72,6 +73,7 @@ set(dependencies
tf2_ros
tf2_sensor_msgs
visualization_msgs
angles
)

ament_target_dependencies(nav2_costmap_2d_core
Expand All @@ -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}
Expand Down
3 changes: 3 additions & 0 deletions nav2_costmap_2d/costmap_plugins.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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>

138 changes: 138 additions & 0 deletions nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp
Original file line number Diff line number Diff line change
@@ -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_
11 changes: 7 additions & 4 deletions nav2_costmap_2d/plugins/inflation_layer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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");
Expand Down Expand Up @@ -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_;

Expand Down Expand Up @@ -260,7 +263,7 @@ InflationLayer::updateCosts(
}
}

for (auto & dist:inflation_cells_) {
for (auto & dist : inflation_cells_) {
dist.clear();
dist.reserve(200);
}
Expand Down
Loading

0 comments on commit a3e0c2d

Please sign in to comment.