zcy
/
ROS2-SLAM-Fundamental
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Initial commit, partially working, lacks explore.cpp

main
charlielito 2021-08-03 19:33:15 -07:00
commit c63a599b1d
18 changed files with 1283 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
.clang-format Normal file
View File

@ -0,0 +1,48 @@
---
BasedOnStyle: Google
AccessModifierOffset: -2
ConstructorInitializerIndentWidth: 2
AlignEscapedNewlinesLeft: false
AlignTrailingComments: true
AllowAllParametersOfDeclarationOnNextLine: false
AllowShortIfStatementsOnASingleLine: false
AllowShortLoopsOnASingleLine: false
AllowShortFunctionsOnASingleLine: None
AllowShortLoopsOnASingleLine: false
AlwaysBreakTemplateDeclarations: true
AlwaysBreakBeforeMultilineStrings: false
BreakBeforeBinaryOperators: false
BreakBeforeTernaryOperators: false
BreakConstructorInitializersBeforeComma: true
BinPackParameters: true
ColumnLimit: 80
ConstructorInitializerAllOnOneLineOrOnePerLine: true
DerivePointerBinding: true
ExperimentalAutoDetectBinPacking: false
IndentCaseLabels: true
MaxEmptyLinesToKeep: 1
NamespaceIndentation: None
ObjCSpaceBeforeProtocolList: true
PenaltyBreakBeforeFirstCallParameter: 19
PenaltyBreakComment: 60
PenaltyBreakString: 1
PenaltyBreakFirstLessLess: 1000
PenaltyExcessCharacter: 100
PenaltyReturnTypeOnItsOwnLine: 90
PointerBindsToType: false
SpacesBeforeTrailingComments: 2
Cpp11BracedListStyle: true
Standard: Cpp11
IndentWidth: 2
TabWidth: 2
UseTab: Never
BreakBeforeBraces: Linux
IndentFunctionDeclarationAfterType: false
SpacesInParentheses: false
SpacesInAngles: false
SpaceInEmptyParentheses: false
SpacesInCStyleCastParentheses: false
SpaceAfterControlStatementKeyword: true
SpaceBeforeAssignmentOperators: true
ContinuationIndentWidth: 4
...

3
.gitignore vendored Normal file
View File

@ -0,0 +1,3 @@
*sublime-*
*.svg
*.xcf

31
LICENSE Normal file
View File

@ -0,0 +1,31 @@
Software License Agreement (BSD License)
Copyright (c) 2015-2016, Carlos Alvarez.
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 Carlos Alvarez 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.

37
README.md Normal file
View File

@ -0,0 +1,37 @@
# m-explore ROS2 port
ROS2 package port for (not yet multi) robot exploration of [m-explore](https://github.com/hrnr/m-explore). Currently tested on Foxy distro.
### TB3
https://user-images.githubusercontent.com/8033598/127044265-f59ee9d1-93c6-4b73-b022-467eeb671d2a.mp4
Installing
----------
No binaries yet.
Building
--------
Build as a standard colcon package. There are no special dependencies needed
(use rosdep to resolve dependencies in ROS). You should use brach specific for
your release i.e. `foxy` for foxy. ROS2 branch is for foxy right now.
RUNNING
-------
To run with a params file just run it with
```
ros2 run explore_lite explore --ros-args --params-file <path_to_ros_ws>/m-explore/explore/config/params.yaml
```
WIKI
----
No wiki yet.
COPYRIGHT
---------
Packages are licensed under BSD license. See respective files for details.

8
explore/CHANGELOG.rst Normal file
View File

@ -0,0 +1,8 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package explore_lite
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.0.0 (2021-08-01)
------------------
* First working port in ros2 foxy
* Contributors: Carlos Alvarez, Juan Gavlis

88
explore/CMakeLists.txt Normal file
View File

@ -0,0 +1,88 @@
cmake_minimum_required(VERSION 3.5)
project(explore_lite)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(nav2_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(map_msgs REQUIRED)
find_package(visualization_msgs REQUIRED)
find_package(nav2_costmap_2d REQUIRED)
set(DEPENDENCIES
rclcpp
std_msgs
sensor_msgs
tf2
tf2_ros
tf2_geometry_msgs
nav2_msgs
nav_msgs
map_msgs
nav2_costmap_2d
visualization_msgs
)
include_directories(
include
)
install(
DIRECTORY include/explore/
DESTINATION include/explore/
)
install(DIRECTORY
config
DESTINATION share/${PROJECT_NAME}
)
install(DIRECTORY
launch
DESTINATION share/${PROJECT_NAME}
)
add_executable(explore
src/costmap_client.cpp
src/explore.cpp
src/frontier_search.cpp
)
target_include_directories(explore PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_link_libraries(explore)
ament_target_dependencies(explore ${DEPENDENCIES})
install(TARGETS explore
DESTINATION lib/${PROJECT_NAME})
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
ament_export_include_directories(include)
ament_package()

13
explore/config/params.yaml Normal file
View File

@ -0,0 +1,13 @@
explore_node:
ros__parameters:
robot_base_frame: base_link
costmap_topic: map
costmap_updates_topic: map_updates
visualize: true
planner_frequency: 0.33
progress_timeout: 30.0
potential_scale: 3.0
orientation_scale: 0.0
gain_scale: 1.0
transform_tolerance: 0.3
min_frontier_size: 0.75

13
explore/config/params_costmap.yaml Normal file
View File

@ -0,0 +1,13 @@
explore_node:
ros__parameters:
robot_base_frame: base_link
costmap_topic: /global_costmap/costmap
costmap_updates_topic: /global_costmap/costmap_updates
visualize: true
planner_frequency: 0.33
progress_timeout: 30.0
potential_scale: 3.0
orientation_scale: 0.0
gain_scale: 1.0
transform_tolerance: 0.3
min_frontier_size: 0.5

BIN
explore/doc/architecture.dia Normal file
View File

Binary file not shown.

BIN
explore/doc/screenshot.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 24 KiB

148
explore/doc/wiki_doc.txt Normal file
View File

@ -0,0 +1,148 @@
<<PackageHeader(explore_lite)>>
<<GitHubIssues(hrnr/m-explore)>>
<<TOC(4)>>
== Overview ==
This package provides greedy frontier-based exploration. When node is running, robot will greedily explore its environment until no frontiers could be found. Movement commands will be send to [[move_base]].
{{attachment:screenshot.png||width="755px"}}
Unlike similar packages, {{{explore_lite}}} does not create its own costmap, which makes it easier to configure and more efficient (lighter on resources). Node simply subscribes to <<MsgLink(nav_msgs/OccupancyGrid)>> messages. Commands for robot movement are send to [[move_base]] node.
Node can do frontier filtering and can operate even on non-inflated maps. Goal blacklisting allows to deal with places inaccessible for robot.
<<Youtube(op0L0LyGNwY&rel=0)>>
== Architecture ==
{{{explore_lite}}} uses [[move_base]] for navigation. You need to run properly configured [[move_base]] node.
{{attachment:architecture.svg||width="755px"}}
{{{explore_lite}}} subscribes to a <<MsgLink(nav_msgs/OccupancyGrid)>> and <<MsgLink(map_msgs/OccupancyGridUpdate)>> messages to construct a map where it looks for frontiers. You can either use costmap published by [[move_base]] (ie. `<move_base>/global_costmap/costmap`) or you can use map constructed by mapping algorithm (SLAM).
Depending on your environment you may achieve better results with either SLAM map or costmap published by `move_base`. Advantage of `move_base` costmap is the inflation which helps to deal with some very small unexplorable frontiers. When you are using a raw map produced by SLAM you should set the `min_frontier_size` parameter to some reasonable number to deal with the small frontiers. For details on both setups check the `explore.launch` and `explore_costmap.launch` launch files.
== Setup ==
Before starting experimenting with {{{explore_lite}}} you need to have working [[move_base]] for navigation. You should be able to navigate with [[move_base]] manually through [[rviz]]. Please refer to [[navigation#Tutorials]] for setting up [[move_base]] and the rest of the navigation stack with your robot.
You should be also able to to navigate with [[move_base]] though unknown space in the map. If you set the goal to unknown place in the map, planning and navigating should work. With most planners this should work by default, refer to [[navfn#Parameters]] if you need to setup this for [[navfn]] planner (but should be enabled by default). Navigation through unknown space is required for {{{explore_lite}}}.
If you want to use costmap provided by [[move_base]] you need to enable unknown space tracking by setting `track_unknown_space: true`.
If you have [[move_base]] configured correctly, you can start experimenting with {{{explore_lite}}}. Provided `explore.launch` should work out-of-the box in most cases, but as always you might need to adjust topic names and frame names according to your setup.
== ROS API ==
{{{
#!clearsilver CS/NodeAPI
name = explore
desc = Provides exploration services offered by this package. Exploration will start immediately after node initialization.
pub {
0.name = ~frontiers
0.type = visualization_msgs/MarkerArray
0.desc = Visualization of frontiers considered by exploring algorithm. Each frontier is visualized by frontier points in blue and with a small sphere, which visualize the cost of the frontiers (costlier frontiers will have smaller spheres).
}
sub {
0.name = costmap
0.type = nav_msgs/OccupancyGrid
0.desc = Map which will be used for exploration planning. Can be either costmap from [[move_base]] or map created by SLAM (see above). Occupancy grid must have got properly marked unknown space, mapping algorithms usually track unknown space by default. If you want to use costmap provided by [[move_base]] you need to enable unknown space tracking by setting `track_unknown_space: true`.
1.name = costmap_updates
1.type = map_msgs/OccupancyGridUpdate
1.desc = Incremental updates on costmap. Not necessary if source of map is always publishing full updates, i.e. does not provide this topic.
}
param {
0.name = ~robot_base_frame
0.default = `base_link`
0.type = string
0.desc = The name of the base frame of the robot. This is used for determining robot position on map. Mandatory.
1.name = ~costmap_topic
1.default = `costmap`
1.type = string
1.desc = Specifies topic of source <<MsgLink(nav_msgs/OccupancyGrid)>>. Mandatory.
3.name = ~costmap_updates_topic
3.default = `costmap_updates`
3.type = string
3.desc = Specifies topic of source <<MsgLink(map_msgs/OccupancyGridUpdate)>>. Not necessary if source of map is always publishing full updates, i.e. does not provide this topic.
4.name = ~visualize
4.default = `false`
4.type = bool
4.desc = Specifies whether or not publish visualized frontiers.
6.name = ~planner_frequency
6.default = `1.0`
6.type = double
6.desc = Rate in Hz at which new frontiers will computed and goal reconsidered.
7.name = ~progress_timeout
7.default = `30.0`
7.type = double
7.desc = Time in seconds. When robot do not make any progress for `progress_timeout`, current goal will be abandoned.
8.name = ~potential_scale
8.default = `1e-3`
8.type = double
8.desc = Used for weighting frontiers. This multiplicative parameter affects frontier potential component of the frontier weight (distance to frontier).
9.name = ~orientation_scale
9.default = `0`
9.type = double
9.desc = Used for weighting frontiers. This multiplicative parameter affects frontier orientation component of the frontier weight. This parameter does currently nothing and is provided solely for forward compatibility.
10.name = ~gain_scale
10.default = `1.0`
10.type = double
10.desc = Used for weighting frontiers. This multiplicative parameter affects frontier gain component of the frontier weight (frontier size).
11.name = ~transform_tolerance
11.default = `0.3`
11.type = double
11.desc = Transform tolerance to use when transforming robot pose.
12.name = ~min_frontier_size
12.default = `0.5`
12.type = double
12.desc = Minimum size of the frontier to consider the frontier as the exploration goal. In meters.
}
req_tf {
0.from = global_frame
0.to = robot_base_frame
0.desc = This transformation is usually provided by mapping algorithm. Those frames are usually called `map` and `base_link`. For adjusting `robot_base_frame` name see respective parameter. You don't need to set `global_frame`. The name for `global_frame` will be sourced from `costmap_topic` automatically.
}
act_called {
0.name = move_base
0.type = move_base_msgs/MoveBaseAction
0.desc = [[move_base]] actionlib API for posting goals. See [[move_base#Action API]] for details. This expects [[move_base]] node in the same namespace as `explore_lite`, you may want to remap this node if this is not true.
}
}}}
== Acknowledgements ==
This package was developed as part of my bachelor thesis at [[http://www.mff.cuni.cz/to.en/|Charles University]] in Prague.
{{{
@masterthesis{Hörner2016,
author = {Jiří Hörner},
title = {Map-merging for multi-robot system},
address = {Prague},
year = {2016},
school = {Charles University in Prague, Faculty of Mathematics and Physics},
type = {Bachelor's thesis},
URL = {https://is.cuni.cz/webapps/zzp/detail/174125/},
}
}}}
This project was initially based on [[explore]] package by Charles !DuHadway. Most of the node has been rewritten since then. The current frontier search algorithm is based on [[frontier_exploration]] by Paul Bovbel.
## AUTOGENERATED DON'T DELETE
## CategoryPackage

136
explore/include/explore/costmap_client.h Normal file
View File

@ -0,0 +1,136 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2015-2016, Jiri Horner, Carlos Alvarez.
* 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 Jiri Horner/Carlos Alvarez 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.
*
*********************************************************************/
#ifndef COSTMAP_CLIENT_
#define COSTMAP_CLIENT_
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <map_msgs/msg/occupancy_grid_update.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <rclcpp/rclcpp.hpp>
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
namespace explore
{
class Costmap2DClient
{
public:
/**
* @brief Contructs client and start listening
* @details Constructor will block until first map update is received and
* map is ready to use, also will block before trasformation
* robot_base_frame <-> global_frame is available.
*
* @param node node handle to retrieve parameters from
* @param tf_listener Will be used for transformation of robot pose.
*/
Costmap2DClient(rclcpp::Node& node, const tf2_ros::Buffer* tf_listener);
/**
* @brief Get the pose of the robot in the global frame of the costmap
* @return pose of the robot in the global frame of the costmap
*/
geometry_msgs::msg::Pose getRobotPose() const;
/**
* @brief Return a pointer to the "master" costmap which receives updates from
* all the layers.
*
* This pointer will stay the same for the lifetime of Costmap2DClient object.
*/
nav2_costmap_2d::Costmap2D* getCostmap()
{
return &costmap_;
}
/**
* @brief Return a pointer to the "master" costmap which receives updates from
* all the layers.
*
* This pointer will stay the same for the lifetime of Costmap2DClient object.
*/
const nav2_costmap_2d::Costmap2D* getCostmap() const
{
return &costmap_;
}
/**
* @brief Returns the global frame of the costmap
* @return The global frame of the costmap
*/
const std::string& getGlobalFrameID() const
{
return global_frame_;
}
/**
* @brief Returns the local frame of the costmap
* @return The local frame of the costmap
*/
const std::string& getBaseFrameID() const
{
return robot_base_frame_;
}
protected:
void updateFullMap(const nav_msgs::msg::OccupancyGrid::SharedPtr msg);
void updatePartialMap(const map_msgs::msg::OccupancyGridUpdate::SharedPtr msg);
nav2_costmap_2d::Costmap2D costmap_;
bool costmap_received_ = false; ///< @brief Flag indicating whether costmap
///< callback has been called
const tf2_ros::Buffer* const tf_; ///< @brief Used for transforming
/// point clouds
rclcpp::Node& node_;
std::string global_frame_; ///< @brief The global frame for the costmap
std::string robot_base_frame_; ///< @brief The frame_id of the robot base
double transform_tolerance_; ///< timeout before transform errors
private:
// will be unsubscribed at destruction
rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr costmap_sub_;
rclcpp::Subscription<map_msgs::msg::OccupancyGridUpdate>::SharedPtr
costmap_updates_sub_;
};
} // namespace explore
#endif

135
explore/include/explore/costmap_tools.h Normal file
View File

@ -0,0 +1,135 @@
#ifndef COSTMAP_TOOLS_H_
#define COSTMAP_TOOLS_H_
#include <geometry_msgs/msg/point_stamped.hpp>
#include <geometry_msgs/msg/polygon_stamped.hpp>
#include <rclcpp/rclcpp.hpp>
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
namespace frontier_exploration
{
/**
* @brief Determine 4-connected neighbourhood of an input cell, checking for map
* edges
* @param idx input cell index
* @param costmap Reference to map data
* @return neighbour cell indexes
*/
std::vector<unsigned int> nhood4(unsigned int idx,
const nav2_costmap_2d::Costmap2D& costmap)
{
// get 4-connected neighbourhood indexes, check for edge of map
std::vector<unsigned int> out;
unsigned int size_x_ = costmap.getSizeInCellsX(),
size_y_ = costmap.getSizeInCellsY();
if (idx > size_x_ * size_y_ - 1) {
RCLCPP_WARN(rclcpp::get_logger("FrontierExploration"), "Evaluating nhood "
"for offmap point");
return out;
}
if (idx % size_x_ > 0) {
out.push_back(idx - 1);
}
if (idx % size_x_ < size_x_ - 1) {
out.push_back(idx + 1);
}
if (idx >= size_x_) {
out.push_back(idx - size_x_);
}
if (idx < size_x_ * (size_y_ - 1)) {
out.push_back(idx + size_x_);
}
return out;
}
/**
* @brief Determine 8-connected neighbourhood of an input cell, checking for map
* edges
* @param idx input cell index
* @param costmap Reference to map data
* @return neighbour cell indexes
*/
std::vector<unsigned int> nhood8(unsigned int idx,
const nav2_costmap_2d::Costmap2D& costmap)
{
// get 8-connected neighbourhood indexes, check for edge of map
std::vector<unsigned int> out = nhood4(idx, costmap);
unsigned int size_x_ = costmap.getSizeInCellsX(),
size_y_ = costmap.getSizeInCellsY();
if (idx > size_x_ * size_y_ - 1) {
return out;
}
if (idx % size_x_ > 0 && idx >= size_x_) {
out.push_back(idx - 1 - size_x_);
}
if (idx % size_x_ > 0 && idx < size_x_ * (size_y_ - 1)) {
out.push_back(idx - 1 + size_x_);
}
if (idx % size_x_ < size_x_ - 1 && idx >= size_x_) {
out.push_back(idx + 1 - size_x_);
}
if (idx % size_x_ < size_x_ - 1 && idx < size_x_ * (size_y_ - 1)) {
out.push_back(idx + 1 + size_x_);
}
return out;
}
/**
* @brief Find nearest cell of a specified value
* @param result Index of located cell
* @param start Index initial cell to search from
* @param val Specified value to search for
* @param costmap Reference to map data
* @return True if a cell with the requested value was found
*/
bool nearestCell(unsigned int& result, unsigned int start, unsigned char val,
const nav2_costmap_2d::Costmap2D& costmap)
{
const unsigned char* map = costmap.getCharMap();
const unsigned int size_x = costmap.getSizeInCellsX(),
size_y = costmap.getSizeInCellsY();
if (start >= size_x * size_y) {
return false;
}
// initialize breadth first search
std::queue<unsigned int> bfs;
std::vector<bool> visited_flag(size_x * size_y, false);
// push initial cell
bfs.push(start);
visited_flag[start] = true;
// search for neighbouring cell matching value
while (!bfs.empty()) {
unsigned int idx = bfs.front();
bfs.pop();
// return if cell of correct value is found
if (map[idx] == val) {
result = idx;
return true;
}
// iterate over all adjacent unvisited cells
for (unsigned nbr : nhood8(idx, costmap)) {
if (!visited_flag[nbr]) {
bfs.push(nbr);
visited_flag[nbr] = true;
}
}
}
return false;
}
} // namespace frontier_exploration
#endif

88
explore/include/explore/frontier_search.h Normal file
View File

@ -0,0 +1,88 @@
#ifndef FRONTIER_SEARCH_H_
#define FRONTIER_SEARCH_H_
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
namespace frontier_exploration
{
/**
* @brief Represents a frontier
*
*/
struct Frontier {
std::uint32_t size;
double min_distance;
double cost;
geometry_msgs::msg::Point initial;
geometry_msgs::msg::Point centroid;
geometry_msgs::msg::Point middle;
std::vector<geometry_msgs::msg::Point> points;
};
/**
* @brief Thread-safe implementation of a frontier-search task for an input
* costmap.
*/
class FrontierSearch
{
public:
FrontierSearch()
{
}
/**
* @brief Constructor for search task
* @param costmap Reference to costmap data to search.
*/
FrontierSearch(nav2_costmap_2d::Costmap2D* costmap, double potential_scale,
double gain_scale, double min_frontier_size);
/**
* @brief Runs search implementation, outward from the start position
* @param position Initial position to search from
* @return List of frontiers, if any
*/
std::vector<Frontier> searchFrom(geometry_msgs::msg::Point position);
protected:
/**
* @brief Starting from an initial cell, build a frontier from valid adjacent
* cells
* @param initial_cell Index of cell to start frontier building
* @param reference Reference index to calculate position from
* @param frontier_flag Flag vector indicating which cells are already marked
* as frontiers
* @return new frontier
*/
Frontier buildNewFrontier(unsigned int initial_cell, unsigned int reference,
std::vector<bool>& frontier_flag);
/**
* @brief isNewFrontierCell Evaluate if candidate cell is a valid candidate
* for a new frontier.
* @param idx Index of candidate cell
* @param frontier_flag Flag vector indicating which cells are already marked
* as frontiers
* @return true if the cell is frontier cell
*/
bool isNewFrontierCell(unsigned int idx,
const std::vector<bool>& frontier_flag);
/**
* @brief computes frontier cost
* @details cost function is defined by potential_scale and gain_scale
*
* @param frontier frontier for which compute the cost
* @return cost of the frontier
*/
double frontierCost(const Frontier& frontier);
private:
nav2_costmap_2d::Costmap2D* costmap_;
unsigned char* map_;
unsigned int size_x_, size_y_;
double potential_scale_, gain_scale_;
double min_frontier_size_;
};
} // namespace frontier_exploration
#endif

21
explore/launch/explore.launch.py Normal file
View File

@ -0,0 +1,21 @@
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
ld = LaunchDescription()
config = os.path.join(
get_package_share_directory("explore_lite"), "config", "params.yaml"
)
node = Node(
package="explore_lite",
name="explore_node",
executable="explore",
parameters=[config],
output="screen",
)
ld.add_action(node)
return ld

33
explore/package.xml Normal file
View File

@ -0,0 +1,33 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>explore_lite</name>
<version>1.0.0</version>
<description>Lightweight frontier-based exploration ROS2 port.</description>
<author email="candres.alv@gmail.com">Carlos Alvarez</author>
<maintainer email="candres.alv@gmail.com">Carlos Alvarez</maintainer>
<license>BSD</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<depend>rclcpp</depend>
<depend>std_msgs</depend>
<depend>sensor_msgs</depend>
<depend>utils</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tf2_geometry_msgs</depend>
<depend>nav_msgs</depend>
<depend>map_msgs</depend>
<depend>visualization_msgs</depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

280
explore/src/costmap_client.cpp Normal file
View File

@ -0,0 +1,280 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2015-2016, Jiri Horner. Carlos Alvarez.
* 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 Jiri Horner/Carlos Alvarez nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*********************************************************************/
#include <explore/costmap_client.h>
#include <functional>
#include <mutex>
#include <string>
namespace explore
{
// static translation table to speed things up
std::array<unsigned char, 256> init_translation_table();
static const std::array<unsigned char, 256> cost_translation_table__ =
init_translation_table();
Costmap2DClient::Costmap2DClient(rclcpp::Node& node, const tf2_ros::Buffer* tf)
: tf_(tf), node_(node)
{
std::string costmap_topic;
std::string costmap_updates_topic;
node_.declare_parameter<std::string>("costmap_topic", std::string("costmap"));
node_.declare_parameter<std::string>("costmap_updates_topic",
std::string("costmap_updates"));
node_.declare_parameter<std::string>("robot_base_frame", std::string("base_"
"link"));
// transform tolerance is used for all tf transforms here
node_.declare_parameter<double>("transform_tolerance", 0.3);
node_.get_parameter("costmap_topic", costmap_topic);
node_.get_parameter("costmap_updates_topic", costmap_updates_topic);
node_.get_parameter("robot_base_frame", robot_base_frame_);
node_.get_parameter("transform_tolerance", transform_tolerance_);
/* initialize costmap */
costmap_sub_ = node_.create_subscription<nav_msgs::msg::OccupancyGrid>(
costmap_topic, 1000,
[this](const nav_msgs::msg::OccupancyGrid::SharedPtr msg) {
costmap_received_ = true;
updateFullMap(msg);
});
// ROS1 CODE
// auto costmap_msg =
// ros::topic::waitForMessage<nav_msgs::msg::OccupancyGrid>(
// costmap_topic, subscription_nh);
// Spin some until the callback gets called to replicate
// ros::topic::waitForMessage
RCLCPP_INFO(node_.get_logger(),
"Waiting for costmap to become available, topic: %s",
costmap_topic.c_str());
while (!costmap_received_) {
rclcpp::spin_some(node_.get_node_base_interface());
// Wait for a second
usleep(1000000);
}
// updateFullMap(costmap_msg); // this is already called in the callback of
// the costmap_sub_
/* subscribe to map updates */
costmap_updates_sub_ =
node_.create_subscription<map_msgs::msg::OccupancyGridUpdate>(
costmap_updates_topic, 1000,
[this](const map_msgs::msg::OccupancyGridUpdate::SharedPtr msg) {
updatePartialMap(msg);
});
// ROS1 CODE.
// TODO: Do we need this?
/* resolve tf prefix for robot_base_frame */
// std::string tf_prefix = tf::getPrefixParam(node_);
// robot_base_frame_ = tf::resolve(tf_prefix, robot_base_frame_);
// we need to make sure that the transform between the robot base frame and
// the global frame is available
// the global frame is set in the costmap callback. This is why we need to
// ensure that a costmap is received
/* tf transform is necessary for getRobotPose */
auto last_error = node_.now();
std::string tf_error;
while (rclcpp::ok() &&
!tf_->canTransform(global_frame_, robot_base_frame_,
tf2::TimePointZero, tf2::durationFromSec(0.1),
&tf_error)) {
rclcpp::spin_some(node_.get_node_base_interface());
if (last_error + tf2::durationFromSec(5.0) < node_.now()) {
RCLCPP_WARN(node_.get_logger(),
"Timed out waiting for transform from %s to %s to become "
"available "
"before subscribing to costmap, tf error: %s",
robot_base_frame_.c_str(), global_frame_.c_str(),
tf_error.c_str());
last_error = node_.now();
;
}
// The error string will accumulate and errors will typically be the same,
// so the last
// will do for the warning above. Reset the string here to avoid
// accumulation.
tf_error.clear();
}
}
void Costmap2DClient::updateFullMap(
const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
{
global_frame_ = msg->header.frame_id;
unsigned int size_in_cells_x = msg->info.width;
unsigned int size_in_cells_y = msg->info.height;
double resolution = msg->info.resolution;
double origin_x = msg->info.origin.position.x;
double origin_y = msg->info.origin.position.y;
RCLCPP_DEBUG(node_.get_logger(), "received full new map, resizing to: %d, %d",
size_in_cells_x, size_in_cells_y);
costmap_.resizeMap(size_in_cells_x, size_in_cells_y, resolution, origin_x,
origin_y);
// lock as we are accessing raw underlying map
auto* mutex = costmap_.getMutex();
// TODO: fix compilation error in following line
// std::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*mutex);
// fill map with data
unsigned char* costmap_data = costmap_.getCharMap();
size_t costmap_size = costmap_.getSizeInCellsX() * costmap_.getSizeInCellsY();
RCLCPP_DEBUG(node_.get_logger(), "full map update, %lu values", costmap_size);
for (size_t i = 0; i < costmap_size && i < msg->data.size(); ++i) {
unsigned char cell_cost = static_cast<unsigned char>(msg->data[i]);
costmap_data[i] = cost_translation_table__[cell_cost];
}
RCLCPP_DEBUG(node_.get_logger(), "map updated, written %lu values",
costmap_size);
}
void Costmap2DClient::updatePartialMap(
const map_msgs::msg::OccupancyGridUpdate::SharedPtr msg)
{
RCLCPP_DEBUG(node_.get_logger(), "received partial map update");
global_frame_ = msg->header.frame_id;
if (msg->x < 0 || msg->y < 0) {
RCLCPP_DEBUG(node_.get_logger(),
"negative coordinates, invalid update. x: %d, y: %d", msg->x,
msg->y);
return;
}
size_t x0 = static_cast<size_t>(msg->x);
size_t y0 = static_cast<size_t>(msg->y);
size_t xn = msg->width + x0;
size_t yn = msg->height + y0;
// lock as we are accessing raw underlying map
auto* mutex = costmap_.getMutex();
// TODO: fix compilation error in following line
// std::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*mutex);
size_t costmap_xn = costmap_.getSizeInCellsX();
size_t costmap_yn = costmap_.getSizeInCellsY();
if (xn > costmap_xn || x0 > costmap_xn || yn > costmap_yn ||
y0 > costmap_yn) {
RCLCPP_WARN(node_.get_logger(),
"received update doesn't fully fit into existing map, "
"only part will be copied. received: [%lu, %lu], [%lu, %lu] "
"map is: [0, %lu], [0, %lu]",
x0, xn, y0, yn, costmap_xn, costmap_yn);
}
// update map with data
unsigned char* costmap_data = costmap_.getCharMap();
size_t i = 0;
for (size_t y = y0; y < yn && y < costmap_yn; ++y) {
for (size_t x = x0; x < xn && x < costmap_xn; ++x) {
size_t idx = costmap_.getIndex(x, y);
unsigned char cell_cost = static_cast<unsigned char>(msg->data[i]);
costmap_data[idx] = cost_translation_table__[cell_cost];
++i;
}
}
}
geometry_msgs::msg::Pose Costmap2DClient::getRobotPose() const
{
geometry_msgs::msg::PoseStamped robot_pose;
robot_pose.header.frame_id = robot_base_frame_;
robot_pose.header.stamp = node_.now();
auto& clk = *node_.get_clock();
// get the global pose of the robot
try {
robot_pose = tf_->transform(robot_pose, global_frame_,
tf2::durationFromSec(transform_tolerance_));
} catch (tf2::LookupException& ex) {
RCLCPP_ERROR_THROTTLE(node_.get_logger(), clk, 1000,
"No Transform available Error looking up robot pose: "
"%s\n",
ex.what());
return {};
} catch (tf2::ConnectivityException& ex) {
RCLCPP_ERROR_THROTTLE(node_.get_logger(), clk, 1000,
"Connectivity Error looking up robot pose: %s\n",
ex.what());
return {};
} catch (tf2::ExtrapolationException& ex) {
RCLCPP_ERROR_THROTTLE(node_.get_logger(), clk, 1000,
"Extrapolation Error looking up robot pose: %s\n",
ex.what());
return {};
} catch (tf2::TransformException& ex) {
RCLCPP_ERROR_THROTTLE(node_.get_logger(), clk, 1000, "Other error: %s\n",
ex.what());
return {};
}
return robot_pose.pose;
}
std::array<unsigned char, 256> init_translation_table()
{
std::array<unsigned char, 256> cost_translation_table;
// lineary mapped from [0..100] to [0..255]
for (size_t i = 0; i < 256; ++i) {
cost_translation_table[i] =
static_cast<unsigned char>(1 + (251 * (i - 1)) / 97);
}
// special values:
cost_translation_table[0] = 0; // NO obstacle
cost_translation_table[99] = 253; // INSCRIBED obstacle
cost_translation_table[100] = 254; // LETHAL obstacle
cost_translation_table[static_cast<unsigned char>(-1)] = 255; // UNKNOWN
return cost_translation_table;
}
} // namespace explore

201
explore/src/frontier_search.cpp Normal file
View File

@ -0,0 +1,201 @@
#include <explore/costmap_tools.h>
#include <explore/frontier_search.h>
#include <geometry_msgs/msg/point.hpp>
#include <mutex>
#include "nav2_costmap_2d/cost_values.hpp"
namespace frontier_exploration
{
using nav2_costmap_2d::FREE_SPACE;
using nav2_costmap_2d::LETHAL_OBSTACLE;
using nav2_costmap_2d::NO_INFORMATION;
FrontierSearch::FrontierSearch(nav2_costmap_2d::Costmap2D* costmap,
double potential_scale, double gain_scale,
double min_frontier_size)
: costmap_(costmap)
, potential_scale_(potential_scale)
, gain_scale_(gain_scale)
, min_frontier_size_(min_frontier_size)
{
}
std::vector<Frontier>
FrontierSearch::searchFrom(geometry_msgs::msg::Point position)
{
std::vector<Frontier> frontier_list;
// Sanity check that robot is inside costmap bounds before searching
unsigned int mx, my;
if (!costmap_->worldToMap(position.x, position.y, mx, my)) {
RCLCPP_ERROR(rclcpp::get_logger("FrontierSearch"), "Robot out of costmap "
"bounds, cannot search "
"for frontiers");
return frontier_list;
}
// make sure map is consistent and locked for duration of search
std::lock_guard<nav2_costmap_2d::Costmap2D::mutex_t> lock(
*(costmap_->getMutex()));
map_ = costmap_->getCharMap();
size_x_ = costmap_->getSizeInCellsX();
size_y_ = costmap_->getSizeInCellsY();
// initialize flag arrays to keep track of visited and frontier cells
std::vector<bool> frontier_flag(size_x_ * size_y_, false);
std::vector<bool> visited_flag(size_x_ * size_y_, false);
// initialize breadth first search
std::queue<unsigned int> bfs;
// find closest clear cell to start search
unsigned int clear, pos = costmap_->getIndex(mx, my);
if (nearestCell(clear, pos, FREE_SPACE, *costmap_)) {
bfs.push(clear);
} else {
bfs.push(pos);
RCLCPP_WARN(rclcpp::get_logger("FrontierSearch"), "Could not find nearby "
"clear cell to start "
"search");
}
visited_flag[bfs.front()] = true;
while (!bfs.empty()) {
unsigned int idx = bfs.front();
bfs.pop();
// iterate over 4-connected neighbourhood
for (unsigned nbr : nhood4(idx, *costmap_)) {
// add to queue all free, unvisited cells, use descending search in case
// initialized on non-free cell
if (map_[nbr] <= map_[idx] && !visited_flag[nbr]) {
visited_flag[nbr] = true;
bfs.push(nbr);
// check if cell is new frontier cell (unvisited, NO_INFORMATION, free
// neighbour)
} else if (isNewFrontierCell(nbr, frontier_flag)) {
frontier_flag[nbr] = true;
Frontier new_frontier = buildNewFrontier(nbr, pos, frontier_flag);
if (new_frontier.size * costmap_->getResolution() >=
min_frontier_size_) {
frontier_list.push_back(new_frontier);
}
}
}
}
// set costs of frontiers
for (auto& frontier : frontier_list) {
frontier.cost = frontierCost(frontier);
}
std::sort(
frontier_list.begin(), frontier_list.end(),
[](const Frontier& f1, const Frontier& f2) { return f1.cost < f2.cost; });
return frontier_list;
}
Frontier FrontierSearch::buildNewFrontier(unsigned int initial_cell,
unsigned int reference,
std::vector<bool>& frontier_flag)
{
// initialize frontier structure
Frontier output;
output.centroid.x = 0;
output.centroid.y = 0;
output.size = 1;
output.min_distance = std::numeric_limits<double>::infinity();
// record initial contact point for frontier
unsigned int ix, iy;
costmap_->indexToCells(initial_cell, ix, iy);
costmap_->mapToWorld(ix, iy, output.initial.x, output.initial.y);
// push initial gridcell onto queue
std::queue<unsigned int> bfs;
bfs.push(initial_cell);
// cache reference position in world coords
unsigned int rx, ry;
double reference_x, reference_y;
costmap_->indexToCells(reference, rx, ry);
costmap_->mapToWorld(rx, ry, reference_x, reference_y);
while (!bfs.empty()) {
unsigned int idx = bfs.front();
bfs.pop();
// try adding cells in 8-connected neighborhood to frontier
for (unsigned int nbr : nhood8(idx, *costmap_)) {
// check if neighbour is a potential frontier cell
if (isNewFrontierCell(nbr, frontier_flag)) {
// mark cell as frontier
frontier_flag[nbr] = true;
unsigned int mx, my;
double wx, wy;
costmap_->indexToCells(nbr, mx, my);
costmap_->mapToWorld(mx, my, wx, wy);
geometry_msgs::msg::Point point;
point.x = wx;
point.y = wy;
output.points.push_back(point);
// update frontier size
output.size++;
// update centroid of frontier
output.centroid.x += wx;
output.centroid.y += wy;
// determine frontier's distance from robot, going by closest gridcell
// to robot
double distance = sqrt(pow((double(reference_x) - double(wx)), 2.0) +
pow((double(reference_y) - double(wy)), 2.0));
if (distance < output.min_distance) {
output.min_distance = distance;
output.middle.x = wx;
output.middle.y = wy;
}
// add to queue for breadth first search
bfs.push(nbr);
}
}
}
// average out frontier centroid
output.centroid.x /= output.size;
output.centroid.y /= output.size;
return output;
}
bool FrontierSearch::isNewFrontierCell(unsigned int idx,
const std::vector<bool>& frontier_flag)
{
// check that cell is unknown and not already marked as frontier
if (map_[idx] != NO_INFORMATION || frontier_flag[idx]) {
return false;
}
// frontier cells should have at least one cell in 4-connected neighbourhood
// that is free
for (unsigned int nbr : nhood4(idx, *costmap_)) {
if (map_[nbr] == FREE_SPACE) {
return true;
}
}
return false;
}
double FrontierSearch::frontierCost(const Frontier& frontier)
{
return (potential_scale_ * frontier.min_distance *
costmap_->getResolution()) -
(gain_scale_ * frontier.size * costmap_->getResolution());
}
} // namespace frontier_exploration