Initial commit, partially working, lacks explore.cpp
charlielito
commit
c63a599b1d
|
|
@ -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
|
||||||
|
...
|
||||||
|
|
@ -0,0 +1,3 @@
|
||||||
|
*sublime-*
|
||||||
|
*.svg
|
||||||
|
*.xcf
|
||||||
|
|
@ -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.
|
||||||
|
|
@ -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.
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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()
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
Binary file not shown.
Binary file not shown.
|
After Width: | Height: | Size: 24 KiB |
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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>
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
Loading…
Reference in New Issue