zcy
/
m-explore-ros2
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

PORT: multirobot_map_merge (#8) 

* Baby steps

* Compile without implementations

* Compile combined_grids

* Compile part of principal node map_merge.cpp

* Finish 90% of main node

* Move ros::names implementation to own file

* Replace ros1 while loops threads with ros2 timers

* Fix checking of map type for adding robots. bypass mutex for now

* Working version without init poses, still verbose logs

* Add namespace to explore_lite
Add tb3 launch files for multirobot tests
Finish multirobot map merge with poses known

* Add files for tb3 demo

* Fix launch file for demo with robot poses

* Fix comments on locking

* Change deprecated Ptr to SharedPtr in ros2 msgs variables

* Change ConstPtr to ConstSharedPtr for ROS2

* Fix minor warnings in explor_lite

* Attempt fix slam toolbox

* Add launch and remove old ones

* Add copyright

* Add readme with instruction on how to run the demo in sim

* Fix readme and port from_map_server launch file

* Fix QoS for suscriptions with TRansientlocal
main
Carlos Andrés Álvarez Restrepo 2021-12-26 20:28:18 -05:00 committed by GitHub
parent 8005841a07
commit f808bc404a
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
35 changed files with 4363 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
README.md
View File

@ -1,7 +1,8 @@
# m-explore ROS2 port # 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 Eloquent, Dashing, Foxy, and Galactic distros. ROS2 package port for multi robot exploration of [m-explore](https://github.com/hrnr/m-explore). Currently tested on Eloquent, Dashing, Foxy, and Galactic distros.
## Autonomous exploration
### TB3 ### TB3
https://user-images.githubusercontent.com/8033598/128805356-be90a880-16c6-4fc9-8f54-e3302873dc8c.mp4 https://user-images.githubusercontent.com/8033598/128805356-be90a880-16c6-4fc9-8f54-e3302873dc8c.mp4
@ -11,7 +12,6 @@ https://user-images.githubusercontent.com/8033598/128805356-be90a880-16c6-4fc9-8
https://user-images.githubusercontent.com/18732666/128493567-6841dde0-2250-4d81-9bcb-8b216e0fb34d.mp4 https://user-images.githubusercontent.com/18732666/128493567-6841dde0-2250-4d81-9bcb-8b216e0fb34d.mp4
Installing Installing
---------- ----------
@ -32,8 +32,12 @@ ros2 run explore_lite explore --ros-args --params-file <path_to_ros_ws>/m-explor
### Running the demo with TB3 ### Running the demo with TB3
Install nav2 and tb3 simulation. You can follow the [tutorial](https://navigation.ros.org/getting_started/index.html#installation). Install nav2 and tb3 simulation. You can follow the [tutorial](https://navigation.ros.org/getting_started/index.html#installation).
Then just run the nav2 stack with slam: Then just run the nav2 stack with slam:
``` ```
export TURTLEBOT3_MODEL=waffle
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/${ROS_DISTRO}/share/turtlebot3_gazebo/models
ros2 launch nav2_bringup tb3_simulation_launch.py slam:=true ros2 launch nav2_bringup tb3_simulation_launch.py slam:=true
``` ```
@ -42,8 +46,9 @@ And run this package with
ros2 launch explore_lite explore.launch.py ros2 launch explore_lite explore.launch.py
``` ```
You can open an rviz2 and add the exploration frontiers marker to see the algorithm working and choose a frontier to explore.
#### TB3 troubleshooting #### TB3 troubleshooting (with foxy)
If you have trouble with TB3 in simulation like we did, add this extra steps for configuring it. If you have trouble with TB3 in simulation like we did, add this extra steps for configuring it.
``` ```
@ -53,13 +58,59 @@ sudo rm -rf /opt/ros/${ROS_DISTRO}/share/turtlebot3_simulations
sudo git clone https://github.com/ROBOTIS-GIT/turtlebot3_simulations /opt/ros/${ROS_DISTRO}/share/turtlebot3_simulations sudo git clone https://github.com/ROBOTIS-GIT/turtlebot3_simulations /opt/ros/${ROS_DISTRO}/share/turtlebot3_simulations
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/${ROS_DISTRO}/share/turtlebot3_simulations/turtlebot3_gazebo/models export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/${ROS_DISTRO}/share/turtlebot3_simulations/turtlebot3_gazebo/models
``` ```
Then you'll be able to run it. Then you'll be able to run it.
## Multirobot map merge
This package works with known and unknown initial poses of the robots. It merges the maps of the robots and publishes the merged map. Some results in simulation:
### Known initial poses (best results)
https://user-images.githubusercontent.com/8033598/144522712-c31fb4bb-bb5a-4859-b3e1-8ad665f80696.mp4
### Unknown initial poses
It works better if the robots start very close (< 3 meters) to each other so their relative positions can be calculated properly.
https://user-images.githubusercontent.com/8033598/144522696-517d54fd-74d0-4c55-9aca-f1b9679afb3e.mp4
### ROS2 requirements
#### SLAM
Because of the logic that merges the maps, currently as a straight forward port to ROS2 from the ROS1 version, the SLAM needs to be done using the ROS1 defacto slam option which is [slam_gmapping](https://github.com/ros-perception/slam_gmapping), which hasn't ported officially to ROS2 yet. There is an unofficial port but it lacks to pass a namespace to its launch file. For that, this repo was tested with one of the authors of this package [fork](https://github.com/charlielito/slam_gmapping/tree/feature/namespace_launch). You'll need to git clone to your workspace and build it with colcon.
```
cd <your/ros2_ws/src>
git clone https://github.com/charlielito/slam_gmapping.git --branch feature/namespace_launch
cd ..
colcon build --symlink-install --packages-up-to slam_gmapping
```
#### Nav2 config files
This repo has some config examples and launch files for running this package with 2 TB3 robots and a world with nav2. Nonetheless, they are only compatible with the galactic branch and since some breaking changes were introduced in this branch, if you want to try it with another ros2 distro you'll need to tweak those param files for that nav2's distro version (which shouldn't be hard).
### Running the demo with TB3
First you'll need to launch the whole simulation stack, nav2 stacks and slam stacks per robot. For that just launch::
```
export TURTLEBOT3_MODEL=waffle
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/${ROS_DISTRO}/share/turtlebot3_gazebo/models
ros2 launch multirobot_map_merge multi_tb3_simulation_launch.py slam_gmapping:=True
```
Now run the merging node:
```
ros2 launch multirobot_map_merge map_merge.launch.py
```
By default the demo runs with known initial poses. You can change that by launching again both launch commands with with the flag `known_init_poses:=False`
Then you can start moving each robot with its corresponding rviz2 interface sending nav2 goals. To see the map merged just launch rviz2:
```
rviz2 -d <your/ros2_ws>/src/m-explore-ros2/map_merge/launch/map_merge.rviz
```
WIKI WIKI
---- ----
No wiki yet. No wiki yet.
COPYRIGHT COPYRIGHT
--------- ---------

2
explore/config/params.yaml
View File

@ -1,4 +1,4 @@
explore_node: /**:
ros__parameters: ros__parameters:
robot_base_frame: base_link robot_base_frame: base_link
costmap_topic: map costmap_topic: map

6
explore/include/explore/explore.h
View File

@ -59,11 +59,11 @@
using namespace std::placeholders; using namespace std::placeholders;
#ifdef ELOQUENT #ifdef ELOQUENT
#define ACTION_NAME "/NavigateToPose" #define ACTION_NAME "NavigateToPose"
#elif DASHING #elif DASHING
#define ACTION_NAME "/NavigateToPose" #define ACTION_NAME "NavigateToPose"
#else #else
#define ACTION_NAME "/navigate_to_pose" #define ACTION_NAME "navigate_to_pose"
#endif #endif
namespace explore namespace explore
{ {

15
explore/launch/explore.launch.py
View File

@ -14,18 +14,33 @@ def generate_launch_description():
get_package_share_directory("explore_lite"), "config", "params.yaml" get_package_share_directory("explore_lite"), "config", "params.yaml"
) )
use_sim_time = LaunchConfiguration("use_sim_time") use_sim_time = LaunchConfiguration("use_sim_time")
namespace = LaunchConfiguration("namespace")
declare_use_sim_time_argument = DeclareLaunchArgument( declare_use_sim_time_argument = DeclareLaunchArgument(
"use_sim_time", default_value="true", description="Use simulation/Gazebo clock" "use_sim_time", default_value="true", description="Use simulation/Gazebo clock"
) )
declare_namespace_argument = DeclareLaunchArgument(
"namespace",
default_value="",
description="Namespace for the explore node",
)
# Map fully qualified names to relative ones so the node's namespace can be prepended.
# In case of the transforms (tf), currently, there doesn't seem to be a better alternative
# https://github.com/ros/geometry2/issues/32
# https://github.com/ros/robot_state_publisher/pull/30
remappings = [("/tf", "tf"), ("/tf_static", "tf_static")]
node = Node( node = Node(
package="explore_lite", package="explore_lite",
name="explore_node", name="explore_node",
namespace=namespace,
executable="explore", executable="explore",
parameters=[config, {"use_sim_time": use_sim_time}], parameters=[config, {"use_sim_time": use_sim_time}],
output="screen", output="screen",
remappings=remappings,
) )
ld.add_action(declare_use_sim_time_argument) ld.add_action(declare_use_sim_time_argument)
ld.add_action(declare_namespace_argument)
ld.add_action(node) ld.add_action(node)
return ld return ld

17
explore/src/costmap_client.cpp
View File

@ -158,9 +158,7 @@ void Costmap2DClient::updateFullMap(
// lock as we are accessing raw underlying map // lock as we are accessing raw underlying map
auto* mutex = costmap_.getMutex(); auto* mutex = costmap_.getMutex();
std::lock_guard<nav2_costmap_2d::Costmap2D::mutex_t> lock(*mutex);
// TODO: fix compilation error in following line
// std::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*mutex);
// fill map with data // fill map with data
unsigned char* costmap_data = costmap_.getCharMap(); unsigned char* costmap_data = costmap_.getCharMap();
@ -194,9 +192,7 @@ void Costmap2DClient::updatePartialMap(
// lock as we are accessing raw underlying map // lock as we are accessing raw underlying map
auto* mutex = costmap_.getMutex(); auto* mutex = costmap_.getMutex();
std::lock_guard<nav2_costmap_2d::Costmap2D::mutex_t> lock(*mutex);
// 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_xn = costmap_.getSizeInCellsX();
size_t costmap_yn = costmap_.getSizeInCellsY(); size_t costmap_yn = costmap_.getSizeInCellsY();
@ -226,6 +222,7 @@ void Costmap2DClient::updatePartialMap(
geometry_msgs::msg::Pose Costmap2DClient::getRobotPose() const geometry_msgs::msg::Pose Costmap2DClient::getRobotPose() const
{ {
geometry_msgs::msg::PoseStamped robot_pose; geometry_msgs::msg::PoseStamped robot_pose;
geometry_msgs::msg::Pose empty_pose;
robot_pose.header.frame_id = robot_base_frame_; robot_pose.header.frame_id = robot_base_frame_;
robot_pose.header.stamp = node_.now(); robot_pose.header.stamp = node_.now();
@ -240,21 +237,21 @@ geometry_msgs::msg::Pose Costmap2DClient::getRobotPose() const
"No Transform available Error looking up robot pose: " "No Transform available Error looking up robot pose: "
"%s\n", "%s\n",
ex.what()); ex.what());
return {}; return empty_pose;
} catch (tf2::ConnectivityException& ex) { } catch (tf2::ConnectivityException& ex) {
RCLCPP_ERROR_THROTTLE(node_.get_logger(), clk, 1000, RCLCPP_ERROR_THROTTLE(node_.get_logger(), clk, 1000,
"Connectivity Error looking up robot pose: %s\n", "Connectivity Error looking up robot pose: %s\n",
ex.what()); ex.what());
return {}; return empty_pose;
} catch (tf2::ExtrapolationException& ex) { } catch (tf2::ExtrapolationException& ex) {
RCLCPP_ERROR_THROTTLE(node_.get_logger(), clk, 1000, RCLCPP_ERROR_THROTTLE(node_.get_logger(), clk, 1000,
"Extrapolation Error looking up robot pose: %s\n", "Extrapolation Error looking up robot pose: %s\n",
ex.what()); ex.what());
return {}; return empty_pose;
} catch (tf2::TransformException& ex) { } catch (tf2::TransformException& ex) {
RCLCPP_ERROR_THROTTLE(node_.get_logger(), clk, 1000, "Other error: %s\n", RCLCPP_ERROR_THROTTLE(node_.get_logger(), clk, 1000, "Other error: %s\n",
ex.what()); ex.what());
return {}; return empty_pose;
} }
return robot_pose.pose; return robot_pose.pose;

10
explore/src/explore.cpp
View File

@ -131,8 +131,14 @@ void Explore::visualizeFrontiers(
m.color.b = 255; m.color.b = 255;
m.color.a = 255; m.color.a = 255;
// lives forever // lives forever
// m.lifetime = ros::Duration(0); #ifdef ELOQUENT
m.lifetime = rclcpp::Duration(0); m.lifetime = rclcpp::Duration(0); // deprecated in galactic warning
#elif DASHING
m.lifetime = rclcpp::Duration(0); // deprecated in galactic warning
#else
m.lifetime = rclcpp::Duration::from_seconds(0); // foxy onwards
#endif
// m.lifetime = rclcpp::Duration::from_nanoseconds(0); // suggested in galactic
m.frame_locked = true; m.frame_locked = true;
// weighted frontiers are always sorted // weighted frontiers are always sorted

58
map_merge/CHANGELOG.rst Normal file
View File

@ -0,0 +1,58 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package multirobot_map_merge
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
2.1.4 (2021-01-07)
------------------
* use C++14
* support both OpenCV 3 and OpenCV 4 (support Debian Buster)
* Contributors: Jiri Horner
2.1.3 (2021-01-03)
------------------
* add missing dependencies to catkin_package calls
* update map_merge for OpenCV 4
* Contributors: Jiri Horner
2.1.2 (2021-01-02)
------------------
* support for ROS Melodic
* bugfix: zero resolution of the merged grid for known initial posiiton
* bugfix: estimation_confidence parameter had no effect
* map_merge: set origin of merged grid in its centre
* map_merge: add new launch file
* map_merge with 2 maps served by map_server
* bugfix: ensure that we never output map with 0 resolution
* bugfix: nullptr derefence while setting resolution of output grid
* Contributors: Jiri Horner
2.1.1 (2017-12-16)
------------------
* fix bugs in CMakeLists.txt: install nodes in packages, so they get shipped in debian packages. fixes `#11 <https://github.com/hrnr/m-explore/issues/11>`_
* map_merge: add bibtex to wiki page
* Contributors: Jiri Horner
2.1.0 (2017-10-30)
------------------
* no major changes. Released together with explore_lite.
2.0.0 (2017-03-26)
------------------
* map_merge: upgrade to package format 2
* node completely rewritten based on my work included in opencv
* uses more reliable features by default -> more robust merging
* known_init_poses is now by default false to make it easy to start for new users
* Contributors: Jiri Horner
1.0.1 (2017-03-25)
------------------
* map_merge: use inverted tranform
* transform needs to be inverted before using
* map_merge: change package description
* we support merging with unknown initial positions
* Contributors: Jiri Horner
1.0.0 (2016-05-11)
------------------
* initial release
* Contributors: Jiri Horner

144
map_merge/CMakeLists.txt Normal file
View File

@ -0,0 +1,144 @@
cmake_minimum_required(VERSION 3.5)
project(multirobot_map_merge)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++17
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
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(geometry_msgs REQUIRED)
find_package(image_geometry REQUIRED)
find_package(map_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(Boost REQUIRED COMPONENTS thread)
# OpenCV is required for merging without initial positions
find_package(OpenCV REQUIRED)
if("${OpenCV_VERSION}" VERSION_LESS "3.0")
message(FATAL_ERROR "This package needs OpenCV >= 3.0")
endif()
if("${OpenCV_VERSION}" VERSION_LESS "4.0")
message(WARNING "This package supports OpenCV 3, but some features may not be "
"available. Upgrade to OpenCV 4 to take advantage of all features.")
endif()
set(DEPENDENCIES
rclcpp
geometry_msgs
image_geometry
map_msgs
nav_msgs
tf2_geometry_msgs
OpenCV
)
## Specify additional locations of header files
include_directories(
# ${Boost_INCLUDE_DIRS}
# ${OpenCV_INCLUDE_DIRS}
include
)
install(
DIRECTORY include/map_merge/
DESTINATION include/map_merge/
)
install(DIRECTORY
launch
config
DESTINATION share/${PROJECT_NAME}
)
# we want static linking for now
add_library(combine_grids STATIC
src/combine_grids/grid_compositor.cpp
src/combine_grids/grid_warper.cpp
src/combine_grids/merging_pipeline.cpp
)
add_executable(map_merge
src/map_merge.cpp
)
#############
## Install ##
#############
target_include_directories(map_merge PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_include_directories(combine_grids PUBLIC
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
"$<INSTALL_INTERFACE:include>"
${rclcpp_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
)
target_link_libraries(combine_grids ${rclcpp_LIBRARIES} ${OpenCV_LIBS})
# target_link_libraries(combine_grids ${OpenCV_LIBRARIES})
target_link_libraries(map_merge combine_grids)
# target_link_libraries(map_merge)
ament_target_dependencies(map_merge ${DEPENDENCIES})
install(
TARGETS combine_grids
EXPORT export_combine_grids
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
INCLUDES DESTINATION include
)
install(TARGETS map_merge
DESTINATION lib/${PROJECT_NAME})
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
ament_export_include_directories(include)
ament_export_libraries(combine_grids)
ament_package()
#############
## Testing ##
#############
# if(CATKIN_ENABLE_TESTING)
# find_package(roslaunch REQUIRED)
# # download test data
# set(base_url https://raw.githubusercontent.com/hrnr/m-explore-extra/master/map_merge)
# catkin_download_test_data(${PROJECT_NAME}_map00.pgm ${base_url}/hector_maps/map00.pgm MD5 915609a85793ec1375f310d44f2daf87)
# catkin_download_test_data(${PROJECT_NAME}_map05.pgm ${base_url}/hector_maps/map05.pgm MD5 cb9154c9fa3d97e5e992592daca9853a)
# catkin_download_test_data(${PROJECT_NAME}_2011-08-09-12-22-52.pgm ${base_url}/gmapping_maps/2011-08-09-12-22-52.pgm MD5 3c2c38e7dec2b7a67f41069ab58badaa)
# catkin_download_test_data(${PROJECT_NAME}_2012-01-28-11-12-01.pgm ${base_url}/gmapping_maps/2012-01-28-11-12-01.pgm MD5 681e704044889c95e47b0c3aadd81f1e)
# catkin_add_gtest(test_merging_pipeline test/test_merging_pipeline.cpp)
# # ensure that test data are downloaded before we run tests
# add_dependencies(test_merging_pipeline ${PROJECT_NAME}_map00.pgm ${PROJECT_NAME}_map05.pgm ${PROJECT_NAME}_2011-08-09-12-22-52.pgm ${PROJECT_NAME}_2012-01-28-11-12-01.pgm)
# target_link_libraries(test_merging_pipeline combine_grids ${catkin_LIBRARIES})
# # test all launch files
# # do not test from_map_server.launch as we don't want to add dependency on map_server and this
# # launchfile is not critical
# roslaunch_add_file_check(launch/map_merge.launch)
# roslaunch_add_file_check(launch/experiments)
# endif()

24
map_merge/config/params.yaml Normal file
View File

@ -0,0 +1,24 @@
map_merge:
ros__parameters:
merging_rate: 4.0
discovery_rate: 0.05
estimation_rate: 0.5
estimation_confidence: 0.6
robot_map_topic: map
robot_map_updates_topic: map_udpates
robot_namespace: ""
merged_map_topic: map
world_frame: world
known_init_poses: true
# known_init_poses: false
# Define here robots' positions in the map if known_init_poses is true
/robot1/map_merge/init_pose_x: 0.0
/robot1/map_merge/init_pose_y: 0.0
/robot1/map_merge/init_pose_z: 0.0
/robot1/map_merge/init_pose_yaw: 0.0
/robot2/map_merge/init_pose_x: -3.0
/robot2/map_merge/init_pose_y: 1.0
/robot2/map_merge/init_pose_z: 0.0
/robot2/map_merge/init_pose_yaw: 0.0

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

Binary file not shown.

BIN
map_merge/doc/screenshot.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 29 KiB

168
map_merge/doc/wiki_doc.txt Normal file
View File

@ -0,0 +1,168 @@
<<PackageHeader(multirobot_map_merge)>>
<<GitHubIssues(hrnr/m-explore)>>
<<TOC(4)>>
== Overview ==
This package provides global map for multiple robots. It can merge maps from arbitrary number of robots. It expects maps from individual robots as ROS topics. If your run multiple robots under the same ROS master then {{{multirobot_map_merge}}} will probably work for you out-of-the-box. It is also very easy to setup an simulation experiment.
{{attachment:screenshot.jpg||width="755px"}}
If your run your robots under multiple ROS masters you need to run your own way of communication between robots and provide maps from robots on local topics (under the same master). Also if you want to distribute merged map back to robots your communication must take care of it.
<<Youtube(8Adrn29BVbM&rel=0)>>
{{{multirobot_map_merge}}} does not depend on any particular communication between robots.
== Architecture ==
{{{multirobot_map_merge}}} finds robot maps dynamically and new robots can be added to system at any time.
{{attachment:architecture.svg||width="755px"}}
To make this dynamic behaviour possible there are some constrains placed on robots. First all robots must publish map under `<robot_namespace>/map`, where topic name (`map`) is configurable, but must be same for all robots. For each robot `<robot_namespace>` will be of cause different.
This node support merging maps with known initial positions of the robots or without. See below for details.
== Merging modes ==
Two merging modes are currently supported as orthogonal options. If you know initial positions of robots you may preferably use the first mode and get exact results (rigid transformation will be computed according to initial positions). If you don't know robot's starting points you are still able to use the second mode where transformation between grids will be determined using heuristic algorithm. You can choose between these two modes using the `known_init_poses` parameter.
=== merging with known initial positions ===
This is preferred mode whenever you are able to determine exact starting point for each robot. You need to provide initial position for each robot. You need to provide set of `<robot_namespace>/map_merge/init_pose` parameters. These positions should be in `world_frame`. See [[#ROS API]].
In this merging these parameters are mandatory. If any of the required parameters is missing robot won't be considered for merging (you will get warning with name of affected robot).
=== merging without known initial positions ===
If you can't provide initial poses for robots this mode has minimal configuration requirements. You need to provide only map topic for each robot. Transformation between grids is estimated by feature-matching algorithm and therefore requires grids to have sufficient amount of overlapping space to make a high-probability match. If grids don't have enough overlapping space to make a solid match, merged map can differ greatly from physical situation.
Estimating transforms between grids is cpu-intesive so you might want to tune `estimation_rate` parameter to run re-estimation less often if it causes any troubles.
== ROS API ==
{{{
#!clearsilver CS/NodeAPI
name = map_merge
desc = Provides map merging services offered by this package. Dynamically looks for new robots in the system and merges their maps.
pub {
0.name = map
0.type = nav_msgs/OccupancyGrid
0.desc = Merged map from all robots in the system.
}
sub {
0.name = <robot_namespace>/map
0.type = nav_msgs/OccupancyGrid
0.desc = Local map for specific robot.
1.name = <robot_namespace>/map_updates
1.type = map_msgs/OccupancyGridUpdate
1.desc = Local map updates for specific robot. Most of the <<MsgLink(nav_msgs/OccupancyGrid)>> sources (mapping algorithms) provides incremental map updates via this topic so they don't need to send always full map. This topic is optional. If your mapping algorithm does not provide this topic it is safe to ignore this topic. However if your mapping algorithm does provide this topic, it is preferable to subscribe to this topic. Otherwise map updates will be slow as all partial updates will be missed and map will be able to update only on full map updates.
}
param {
group.0 {
name = ROBOT PARAMETERS
desc = Parameters that should be defined in the namespace of each robot if you want to use merging with known initial poses of robots (`known_init_poses` is `true`). Without these parameters robots won't be considered for merging. If you can't provide these parameters use merging without known initial poses. See [[#Merging modes]]
0.name = <robot_namespace>/map_merge/init_pose_x
0.default = `<no_default>`
0.type = double
0.desc = `x` coordinate of robot initial position in `world_frame`. Should be in meters. It does not matter which frame you will consider global (preferably it should be different from all robots frames), but relative positions of robots in this frame must be correct.
1.name = <robot_namespace>/map_merge/init_pose_y
1.default = `<no_default>`
1.type = double
1.desc = `y` coordinate of robot initial position in `world_frame`.
2.name = <robot_namespace>/map_merge/init_pose_z
2.default = `<no_default>`
2.type = double
2.desc = `z` coordinate of robot initial position in `world_frame`.
3.name = <robot_namespace>/map_merge/init_pose_yaw
3.default = `<no_default>`
3.type = double
3.desc = `yaw` component of robot initial position in `world_frame`. Represents robot rotation in radians.
}
group.1 {
name = NODE PARAMETERS
desc = Parameters that should be defined in the namespace of this node.
0.name = ~robot_map_topic
0.default = `map`
0.type = string
0.desc = Name of robot map topic without namespaces (last component of topic name). Only topics with this name will be considered when looking for new maps to merge. This topics may be subject to further filtering (see below).
1.name = ~robot_map_updates_topic
1.default = `map_updates`
1.type = string
1.desc = Name of robot map updates topic of <<MsgLink(map_msgs/OccupancyGridUpdate)>> without namespaces (last component of topic name). This topic will be always subscribed in the same namespace as `robot_map_topic`. You'll likely need to change this only when you changed `robot_map_topic`. These topics are never considered when searching for new robots.
2.name = ~robot_namespace
2.default = `<empty string>`
2.type = string
2.desc = Fixed part of robot map topic. You can employ this parameter to further limit which topics will be considered during dynamic lookup for robots. Only topics which contain (anywhere) this string will be considered for lookup. Unlike `robot_map_topic` you are not limited by namespace logic. Topics will be filtered using text-based search. Therefore `robot_namespace` does not need to be ROS namespace, but can contain slashes etc. This must be common part of all robots map topics name (all robots for which you want to merge map).
3.name = ~known_init_poses
3.default = `true`
3.type = bool
3.desc = Selects between merging modes. `true` if merging with known initial positions. See [[#Merging modes]]
4.name = ~merged_map_topic
4.default = `map`
4.type = string
4.desc = Topic name where merged map will be published.
5.name = ~world_frame
5.default = `world`
5.type = string
5.desc = Frame id (in [[tf]] tree) which will be assigned to published merged map. This should be frame where you specified robot initial positions.
6.name = ~merging_rate
6.default = `4.0`
6.type = double
6.desc = Rate in Hz. Basic frequency on which this node discovers merges robots maps and publish merged map. Increase this value if you want faster updates.
7.name = ~discovery_rate
7.default = `0.05`
7.type = double
7.desc = Rate in Hz. Frequency on which this node discovers new robots. Increase this value if you need more agile behaviour when adding new robots. Robots will be discovered sooner.
8.name = ~estimation_rate
8.default = `0.5`
8.type = double
8.desc = Rate in Hz. This parameter is relevant only when merging without known positions, see [[#Merging modes]]. Frequency on which this node re-estimates transformation between grids. Estimation is cpu-intensive, so you may wish to lower this value.
9.name = ~estimation_confidence
9.default = `1.0`
9.type = double
9.desc = This parameter is relevant only when merging without known positions, see [[#Merging modes]]. Confidence according to probabilistic model for initial positions estimation. Default value 1.0 is suitable for most applications, increase this value for more confident estimations. Number of maps included in the merge may decrease with increasing confidence. Generally larger overlaps between maps will be required for map to be included in merge. Good range for tuning is [1.0, 2.0].
}
}
}}}
== 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/},
}
}}}
Idea for dynamic robot discovery is from [[map_merging]] package from Zhi Yan. Merging algorithm and configuration are different.
## AUTOGENERATED DON'T DELETE
## CategoryPackage

60
map_merge/include/combine_grids/grid_compositor.h Normal file
View File

@ -0,0 +1,60 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2015-2016, Jiri Horner.
* Copyright (c) 2021, 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 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 GRID_COMPOSITOR_H_
#define GRID_COMPOSITOR_H_
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <opencv2/core/utility.hpp>
namespace combine_grids
{
namespace internal
{
class GridCompositor
{
public:
nav_msgs::msg::OccupancyGrid::SharedPtr compose(const std::vector<cv::Mat>& grids,
const std::vector<cv::Rect>& rois);
};
} // namespace internal
} // namespace combine_grids
#endif // GRID_COMPOSITOR_H_

61
map_merge/include/combine_grids/grid_warper.h Normal file
View File

@ -0,0 +1,61 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2015-2016, Jiri Horner.
* Copyright (c) 2021, 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 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 GRID_WARPER_H_
#define GRID_WARPER_H_
#include <opencv2/core/utility.hpp>
namespace combine_grids
{
namespace internal
{
class GridWarper
{
public:
cv::Rect warp(const cv::Mat& grid, const cv::Mat& transform,
cv::Mat& warped_grid);
private:
cv::Rect warpRoi(const cv::Mat& grid, const cv::Mat& transform);
};
} // namespace internal
} // namespace combine_grids
#endif // GRID_WARPER_H_

146
map_merge/include/combine_grids/merging_pipeline.h Normal file
View File

@ -0,0 +1,146 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2015-2016, Jiri Horner.
* Copyright (c) 2021, 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 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 MERGING_PIPELINE_H_
#define MERGING_PIPELINE_H_
#include <vector>
#include <geometry_msgs/msg/transform.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <opencv2/core/utility.hpp>
namespace combine_grids
{
enum class FeatureType { AKAZE, ORB, SURF };
/**
* @brief Pipeline for merging overlapping occupancy grids
* @details Pipeline works on internally stored grids.
*/
class MergingPipeline
{
public:
template <typename InputIt>
void feed(InputIt grids_begin, InputIt grids_end);
bool estimateTransforms(FeatureType feature = FeatureType::AKAZE,
double confidence = 1.0);
nav_msgs::msg::OccupancyGrid::SharedPtr composeGrids();
std::vector<geometry_msgs::msg::Transform> getTransforms() const;
template <typename InputIt>
bool setTransforms(InputIt transforms_begin, InputIt transforms_end);
private:
std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> grids_;
std::vector<cv::Mat> images_;
std::vector<cv::Mat> transforms_;
double max_confidence_achieved_ = 0.0;
};
template <typename InputIt>
void MergingPipeline::feed(InputIt grids_begin, InputIt grids_end)
{
static_assert(std::is_assignable<nav_msgs::msg::OccupancyGrid::ConstSharedPtr&,
decltype(*grids_begin)>::value,
"grids_begin must point to nav_msgs::msg::OccupancyGrid::ConstSharedPtr "
"data");
// we can't reserve anything, because we want to support just InputIt and
// their guarantee validity for only single-pass algos
images_.clear();
grids_.clear();
for (InputIt it = grids_begin; it != grids_end; ++it) {
if (*it && !(*it)->data.empty()) {
grids_.push_back(*it);
/* convert to opencv images. it creates only a view for opencv and does
* not copy or own actual data. */
images_.emplace_back((*it)->info.height, (*it)->info.width, CV_8UC1,
const_cast<signed char*>((*it)->data.data()));
} else {
grids_.emplace_back();
images_.emplace_back();
}
}
}
template <typename InputIt>
bool MergingPipeline::setTransforms(InputIt transforms_begin,
InputIt transforms_end)
{
static_assert(std::is_assignable<geometry_msgs::msg::Transform&,
decltype(*transforms_begin)>::value,
"transforms_begin must point to geometry_msgs::msg::Transform "
"data");
decltype(transforms_) transforms_buf;
for (InputIt it = transforms_begin; it != transforms_end; ++it) {
const geometry_msgs::msg::Quaternion& q = it->rotation;
if ((q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w) <
std::numeric_limits<double>::epsilon()) {
// represents invalid transform
transforms_buf.emplace_back();
continue;
}
double s = 2.0 / (q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
double a = 1 - q.y * q.y * s - q.z * q.z * s;
double b = q.x * q.y * s + q.z * q.w * s;
double tx = it->translation.x;
double ty = it->translation.y;
cv::Mat transform = cv::Mat::eye(3, 3, CV_64F);
transform.at<double>(0, 0) = transform.at<double>(1, 1) = a;
transform.at<double>(1, 0) = b;
transform.at<double>(0, 1) = -b;
transform.at<double>(0, 2) = tx;
transform.at<double>(1, 2) = ty;
transforms_buf.emplace_back(std::move(transform));
}
if (transforms_buf.size() != images_.size()) {
return false;
}
std::swap(transforms_, transforms_buf);
return true;
}
} // namespace combine_grids
#endif // MERGING_PIPELINE_H_

128
map_merge/include/map_merge/map_merge.h Normal file
View File

@ -0,0 +1,128 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2014, Zhi Yan.
* Copyright (c) 2015-2016, Jiri Horner.
* Copyright (c) 2021, 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 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 MAP_MERGE_H_
#define MAP_MERGE_H_
#include <atomic>
#include <forward_list>
#include <mutex>
#include <unordered_map>
#include <combine_grids/merging_pipeline.h>
#include <geometry_msgs/msg/transform.hpp>
#include <map_msgs/msg/occupancy_grid_update.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <rclcpp/rclcpp.hpp>
#include <boost/thread.hpp>
namespace map_merge
{
struct MapSubscription {
// protects consistency of writable_map and readonly_map
// also protects reads and writes of shared_ptrs
std::mutex mutex;
geometry_msgs::msg::Transform initial_pose;
nav_msgs::msg::OccupancyGrid::SharedPtr writable_map;
nav_msgs::msg::OccupancyGrid::ConstSharedPtr readonly_map;
// ros::Subscriber map_sub;
// ros::Subscriber map_updates_sub;
rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_sub;
rclcpp::Subscription<map_msgs::msg::OccupancyGridUpdate>::SharedPtr map_updates_sub;
};
class MapMerge : public rclcpp::Node
{
private:
/* parameters */
double merging_rate_;
double discovery_rate_;
double estimation_rate_;
double confidence_threshold_;
std::string robot_map_topic_;
std::string robot_map_updates_topic_;
std::string robot_namespace_;
std::string world_frame_;
bool have_initial_poses_;
// publishing
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr merged_map_publisher_;
// maps robots namespaces to maps. does not own
std::unordered_map<std::string, MapSubscription*> robots_;
// owns maps -- iterator safe
std::forward_list<MapSubscription> subscriptions_;
size_t subscriptions_size_;
boost::shared_mutex subscriptions_mutex_;
combine_grids::MergingPipeline pipeline_;
std::mutex pipeline_mutex_;
rclcpp::Logger logger_ = rclcpp::get_logger("MapMergeNode");
// timers
rclcpp::TimerBase::SharedPtr map_merging_timer_;
rclcpp::TimerBase::SharedPtr topic_subscribing_timer_;
rclcpp::TimerBase::SharedPtr pose_estimation_timer_;
std::string robotNameFromTopic(const std::string& topic);
// bool isRobotMapTopic(const ros::master::TopicInfo& topic);
bool isRobotMapTopic(const std::string topic, std::string type);
bool getInitPose(const std::string& name, geometry_msgs::msg::Transform& pose);
void fullMapUpdate(const nav_msgs::msg::OccupancyGrid::SharedPtr msg,
MapSubscription& map);
void partialMapUpdate(const map_msgs::msg::OccupancyGridUpdate::SharedPtr msg,
MapSubscription& map);
public:
MapMerge();
void topicSubscribing();
void mapMerging();
/**
* @brief Estimates initial positions of grids
* @details Relevant only if initial poses are not known
*/
void poseEstimation();
};
} // namespace map_merge
#endif /* MAP_MERGE_H_ */

141
map_merge/include/map_merge/ros1_names.hpp Normal file
View File

@ -0,0 +1,141 @@
/*
* Copyright (C) 2009, Willow Garage, Inc.
*
* 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 names of Stanford University or Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <ctype.h>
#include <string>
#include <sstream>
// Almost all code was taken from http://docs.ros.org/en/indigo/api/roscpp/html/names_8cpp_source.html
class InvalidNameException : public std::runtime_error
{
public:
InvalidNameException(const std::string& msg)
: std::runtime_error(msg)
{}
};
namespace ros1_names
{
bool isValidCharInName(char c)
{
if (isalnum(c) || c == '/' || c == '_')
{
return true;
}
return false;
}
bool validate(const std::string& name, std::string& error)
{
if (name.empty())
{
return true;
}
// First element is special, can be only ~ / or alpha
char c = name[0];
if (!isalpha(c) && c != '/' && c != '~')
{
std::stringstream ss;
ss << "Character [" << c << "] is not valid as the first character in Graph Resource Name [" << name << "]. Valid characters are a-z, A-Z, / and in some cases ~.";
error = ss.str();
return false;
}
for (size_t i = 1; i < name.size(); ++i)
{
c = name[i];
if (!isValidCharInName(c))
{
std::stringstream ss;
ss << "Character [" << c << "] at element [" << i << "] is not valid in Graph Resource Name [" << name <<"]. Valid characters are a-z, A-Z, 0-9, / and _.";
error = ss.str();
return false;
}
}
return true;
}
std::string parentNamespace(const std::string& name)
{
std::string error;
if (!validate(name, error))
{
throw InvalidNameException(error);
}
if (!name.compare("")) return "";
if (!name.compare("/")) return "/";
std::string stripped_name;
// rstrip trailing slash
if (name.find_last_of('/') == name.size()-1)
stripped_name = name.substr(0, name.size() -2);
else
stripped_name = name;
//pull everything up to the last /
size_t last_pos = stripped_name.find_last_of('/');
if (last_pos == std::string::npos)
{
return "";
}
else if (last_pos == 0)
return "/";
return stripped_name.substr(0, last_pos);
}
std::string clean(const std::string& name)
{
std::string clean = name;
size_t pos = clean.find("//");
while (pos != std::string::npos)
{
clean.erase(pos, 1);
pos = clean.find("//", pos);
}
if (*clean.rbegin() == '/')
{
clean.erase(clean.size() - 1, 1);
}
return clean;
}
std::string append(const std::string& left, const std::string& right)
{
return clean(left + "/" + right);
}
} // namespace ros1_names

141
map_merge/launch/from_map_server.launch.py Normal file
View File

@ -0,0 +1,141 @@
# showcases map_merge with static maps served by map_server
# you can run this with test maps provided in m-explore-extra repo
# https://github.com/hrnr/m-explore-extra
# roslaunch multirobot_map_merge from_map_server.launch map1:=PATH_TO_m-explore-extra/map_merge/gmapping_maps/2011-08-09-12-22-52.yaml map2:=PATH_TO_m-explore-extra/map_merge/gmapping_maps/2012-01-28-11-12-01.yaml rviz:=True
import os
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node, PushRosNamespace
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
EmitEvent,
GroupAction,
RegisterEventHandler,
)
from launch.conditions import IfCondition
from launch.event_handlers import OnProcessExit
from launch.events import Shutdown
from launch.substitutions import LaunchConfiguration
def generate_launch_description():
ld = LaunchDescription()
config = os.path.join(
get_package_share_directory("multirobot_map_merge"), "config", "params.yaml"
)
use_sim_time = LaunchConfiguration("use_sim_time")
namespace = LaunchConfiguration("namespace")
known_init_poses = LaunchConfiguration("known_init_poses")
rviz = LaunchConfiguration("rviz")
declare_use_sim_time_argument = DeclareLaunchArgument(
"use_sim_time", default_value="true", description="Use simulation/Gazebo clock"
)
declare_namespace_argument = DeclareLaunchArgument(
"namespace",
default_value="/",
description="Namespace for the explore node",
)
declare_known_init_poses_argument = DeclareLaunchArgument(
"known_init_poses",
default_value="False",
description="Known initial poses of the robots. If so don't forget to declare them in the params.yaml file",
)
declare_rviz_argument = DeclareLaunchArgument(
"rviz",
default_value="False",
description="Run rviz2",
)
num_maps = 2
group_actions = []
for i in range(num_maps):
map_argument_name = f"map{i+1}"
map_yaml_file = LaunchConfiguration(map_argument_name)
declare_map_argument = DeclareLaunchArgument(
map_argument_name,
default_value=f"{map_argument_name}.yaml",
description="Full path to map yaml file to load",
)
map_server = Node(
package="nav2_map_server",
executable="map_server",
name="map_server",
namespace=f"robot{i+1}",
output="screen",
parameters=[
{"yaml_filename": map_yaml_file},
{"use_sim_time": use_sim_time},
],
)
map_server_manager = Node(
package="nav2_lifecycle_manager",
executable="lifecycle_manager",
name="lifecycle_manager_localization",
output="screen",
namespace=f"robot{i+1}",
parameters=[
{"use_sim_time": use_sim_time},
{"autostart": True},
{"node_names": ["map_server"]},
],
)
group_action = GroupAction(
[
PushRosNamespace(namespace=namespace),
map_server_manager,
map_server,
declare_map_argument,
]
)
group_actions.append(group_action)
node = Node(
package="multirobot_map_merge",
name="map_merge",
namespace=namespace,
executable="map_merge",
parameters=[
config,
{"use_sim_time": use_sim_time},
{"known_init_poses": known_init_poses},
],
output="screen",
)
rviz_config_file = os.path.join(
get_package_share_directory("multirobot_map_merge"), "launch", "map_merge.rviz"
)
start_rviz_cmd = Node(
condition=IfCondition(rviz),
package="rviz2",
executable="rviz2",
arguments=["-d", rviz_config_file],
output="screen",
)
exit_event_handler = RegisterEventHandler(
condition=IfCondition(rviz),
event_handler=OnProcessExit(
target_action=start_rviz_cmd,
on_exit=EmitEvent(event=Shutdown(reason="rviz exited")),
),
)
ld.add_action(declare_use_sim_time_argument)
ld.add_action(declare_known_init_poses_argument)
ld.add_action(declare_namespace_argument)
ld.add_action(declare_rviz_argument)
for group_action in group_actions:
ld.add_action(group_action)
ld.add_action(node)
ld.add_action(start_rviz_cmd)
ld.add_action(exit_event_handler)
return ld

57
map_merge/launch/map_merge.launch.py Normal file
View File

@ -0,0 +1,57 @@
import os
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
def generate_launch_description():
ld = LaunchDescription()
config = os.path.join(
get_package_share_directory("multirobot_map_merge"), "config", "params.yaml"
)
use_sim_time = LaunchConfiguration("use_sim_time")
namespace = LaunchConfiguration("namespace")
known_init_poses = LaunchConfiguration("known_init_poses")
declare_use_sim_time_argument = DeclareLaunchArgument(
"use_sim_time", default_value="true", description="Use simulation/Gazebo clock"
)
declare_namespace_argument = DeclareLaunchArgument(
"namespace",
default_value="/",
description="Namespace for the explore node",
)
declare_known_init_poses_argument = DeclareLaunchArgument(
"known_init_poses",
default_value="True",
description="Known initial poses of the robots. If so don't forget to declare them in the params.yaml file",
)
# Map fully qualified names to relative ones so the node's namespace can be prepended.
# In case of the transforms (tf), currently, there doesn't seem to be a better alternative
# https://github.com/ros/geometry2/issues/32
# https://github.com/ros/robot_state_publisher/pull/30
remappings = [("/tf", "tf"), ("/tf_static", "tf_static")]
node = Node(
package="multirobot_map_merge",
name="map_merge",
namespace=namespace,
executable="map_merge",
parameters=[
config,
{"use_sim_time": use_sim_time},
{"known_init_poses": known_init_poses},
],
output="screen",
remappings=remappings,
)
ld.add_action(declare_use_sim_time_argument)
ld.add_action(declare_known_init_poses_argument)
ld.add_action(declare_namespace_argument)
ld.add_action(node)
return ld

154
map_merge/launch/map_merge.rviz Normal file
View File

@ -0,0 +1,154 @@
Panels:
- Class: rviz_common/Displays
Help Height: 138
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
Splitter Ratio: 0.5
Tree Height: 1123
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /2D Goal Pose1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz_common/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 0.699999988079071
Class: rviz_default_plugins/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: Map
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /map
Update Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /map_updates
Use Timestamp: false
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: world
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/Interact
Hide Inactive Objects: true
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Covariance x: 0.25
Covariance y: 0.25
Covariance yaw: 0.06853891909122467
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /goal_pose
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 33.400630950927734
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.5996218919754028
Y: 0.5541106462478638
Z: -0.0447520911693573
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 1.5697963237762451
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 3.1304056644439697
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1600
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd0000000400000000000001dc00000551fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000ab00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000069000005510000017800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000014f000003aefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000069000003ae0000012300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004ae00000051fc0100000002fb0000000800540069006d00650100000000000004ae0000045300fffffffb0000000800540069006d00650100000000000004500000000000000000000002c60000055100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1198
X: 1322
Y: 115

202
map_merge/launch/tb3_simulation/bringup_launch.py Normal file
View File

@ -0,0 +1,202 @@
# Copyright (c) 2018 Intel Corporation
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
GroupAction,
IncludeLaunchDescription,
SetEnvironmentVariable,
)
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch_ros.actions import PushRosNamespace
def generate_launch_description():
# Get the launch directory
bringup_dir = get_package_share_directory("nav2_bringup")
launch_dir = os.path.join(bringup_dir, "launch")
# Get the launch directory of gmapping
slam_gmapping_dir = get_package_share_directory("slam_gmapping")
slam_gmapping_launch_dir = os.path.join(slam_gmapping_dir, "launch")
# Get the launch directory of map_merge
map_merge_dir = get_package_share_directory("multirobot_map_merge")
map_merge_launch_dir = os.path.join(map_merge_dir, "launch", "tb3_simulation")
# Create the launch configuration variables
namespace = LaunchConfiguration("namespace")
use_namespace = LaunchConfiguration("use_namespace")
slam = LaunchConfiguration("slam")
map_yaml_file = LaunchConfiguration("map")
use_sim_time = LaunchConfiguration("use_sim_time")
params_file = LaunchConfiguration("params_file")
autostart = LaunchConfiguration("autostart")
slam_toolbox = LaunchConfiguration("slam_toolbox")
slam_gmapping = LaunchConfiguration("slam_gmapping")
stdout_linebuf_envvar = SetEnvironmentVariable(
"RCUTILS_LOGGING_BUFFERED_STREAM", "1"
)
declare_namespace_cmd = DeclareLaunchArgument(
"namespace", default_value="", description="Top-level namespace"
)
declare_use_namespace_cmd = DeclareLaunchArgument(
"use_namespace",
default_value="false",
description="Whether to apply a namespace to the navigation stack",
)
declare_slam_cmd = DeclareLaunchArgument(
"slam", default_value="False", description="Whether run a SLAM"
)
declare_slam_toolbox_cmd = DeclareLaunchArgument(
"slam_toolbox", default_value="False", description="Whether run a SLAM toolbox"
)
declare_slam_gmapping_cmd = DeclareLaunchArgument(
"slam_gmapping",
default_value="False",
description="Whether run a SLAM gmapping",
)
declare_map_yaml_cmd = DeclareLaunchArgument(
"map", description="Full path to map yaml file to load"
)
declare_use_sim_time_cmd = DeclareLaunchArgument(
"use_sim_time",
default_value="false",
description="Use simulation (Gazebo) clock if true",
)
declare_params_file_cmd = DeclareLaunchArgument(
"params_file",
default_value=os.path.join(bringup_dir, "params", "nav2_params.yaml"),
description="Full path to the ROS2 parameters file to use for all launched nodes",
)
declare_autostart_cmd = DeclareLaunchArgument(
"autostart",
default_value="true",
description="Automatically startup the nav2 stack",
)
# Specify the actions
bringup_cmd_group = GroupAction(
[
PushRosNamespace(condition=IfCondition(use_namespace), namespace=namespace),
# IncludeLaunchDescription(
# PythonLaunchDescriptionSource(
# os.path.join(launch_dir, "slam_launch.py")
# ),
# condition=IfCondition(
# PythonExpression(
# [slam, " and ", slam_toolbox, " and not ", slam_gmapping]
# )
# ),
# launch_arguments={
# "namespace": namespace,
# "use_sim_time": use_sim_time,
# "autostart": autostart,
# "params_file": params_file,
# }.items(),
# ),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(map_merge_launch_dir, "slam_toolbox.py")
),
condition=IfCondition(
PythonExpression(
[slam, " and ", slam_toolbox, " and not ", slam_gmapping]
)
),
launch_arguments={
"use_sim_time": use_sim_time,
}.items(),
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(launch_dir, "localization_launch.py")
),
condition=IfCondition(PythonExpression(["not ", slam])),
launch_arguments={
"namespace": namespace,
"map": map_yaml_file,
"use_sim_time": use_sim_time,
"autostart": autostart,
"params_file": params_file,
"use_lifecycle_mgr": "false",
}.items(),
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(launch_dir, "navigation_launch.py")
),
launch_arguments={
"namespace": namespace,
"use_sim_time": use_sim_time,
"autostart": autostart,
"params_file": params_file,
"use_lifecycle_mgr": "false",
"map_subscribe_transient_local": "true",
}.items(),
),
]
)
# Not in GroupAction because namespace were prepended twice because
# slam_gmapping.launch.py already accepts a namespace argument
slam_gmapping_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(slam_gmapping_launch_dir, "slam_gmapping.launch.py")
),
condition=IfCondition(
PythonExpression([slam, " and ", slam_gmapping, " and not ", slam_toolbox])
),
launch_arguments={
"namespace": namespace,
"use_sim_time": use_sim_time,
}.items(),
)
# Create the launch description and populate
ld = LaunchDescription()
# Set environment variables
ld.add_action(stdout_linebuf_envvar)
# Declare the launch options
ld.add_action(declare_namespace_cmd)
ld.add_action(declare_use_namespace_cmd)
ld.add_action(declare_slam_cmd)
ld.add_action(declare_slam_toolbox_cmd)
ld.add_action(declare_slam_gmapping_cmd)
ld.add_action(declare_map_yaml_cmd)
ld.add_action(declare_use_sim_time_cmd)
ld.add_action(declare_params_file_cmd)
ld.add_action(declare_autostart_cmd)
# Add the actions to launch all of the navigation nodes
ld.add_action(bringup_cmd_group)
ld.add_action(slam_gmapping_cmd)
return ld

287
map_merge/launch/tb3_simulation/config/nav2_multirobot_params_1.yaml Normal file
View File

@ -0,0 +1,287 @@
amcl:
ros__parameters:
use_sim_time: True
alpha1: 0.2
alpha2: 0.2
alpha3: 0.2
alpha4: 0.2
alpha5: 0.2
base_frame_id: "base_footprint"
beam_skip_distance: 0.5
beam_skip_error_threshold: 0.9
beam_skip_threshold: 0.3
do_beamskip: false
global_frame_id: "map"
lambda_short: 0.1
laser_likelihood_max_dist: 2.0
laser_max_range: 100.0
laser_min_range: -1.0
laser_model_type: "likelihood_field"
max_beams: 60
max_particles: 2000
min_particles: 500
odom_frame_id: "odom"
pf_err: 0.05
pf_z: 0.99
recovery_alpha_fast: 0.0
recovery_alpha_slow: 0.0
resample_interval: 1
robot_model_type: "differential"
save_pose_rate: 0.5
sigma_hit: 0.2
tf_broadcast: true
transform_tolerance: 1.0
update_min_a: 0.2
update_min_d: 0.25
z_hit: 0.5
z_max: 0.05
z_rand: 0.5
z_short: 0.05
scan_topic: scan
amcl_map_client:
ros__parameters:
use_sim_time: True
amcl_rclcpp_node:
ros__parameters:
use_sim_time: True
bt_navigator:
ros__parameters:
use_sim_time: True
global_frame: map
robot_base_frame: base_link
odom_topic: /odom
bt_loop_duration: 10
default_server_timeout: 20
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
# They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
# if enable_groot_monitoring is set to True, ports need to be different for each robot !!
enable_groot_monitoring: False
groot_zmq_publisher_port: 1666
groot_zmq_server_port: 1667
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
- nav2_compute_path_through_poses_action_bt_node
- nav2_follow_path_action_bt_node
- nav2_back_up_action_bt_node
- nav2_spin_action_bt_node
- nav2_wait_action_bt_node
- nav2_clear_costmap_service_bt_node
- nav2_is_stuck_condition_bt_node
- nav2_goal_reached_condition_bt_node
- nav2_goal_updated_condition_bt_node
- nav2_initial_pose_received_condition_bt_node
- nav2_reinitialize_global_localization_service_bt_node
- nav2_rate_controller_bt_node
- nav2_distance_controller_bt_node
- nav2_speed_controller_bt_node
- nav2_truncate_path_action_bt_node
- nav2_goal_updater_node_bt_node
- nav2_recovery_node_bt_node
- nav2_pipeline_sequence_bt_node
- nav2_round_robin_node_bt_node
- nav2_transform_available_condition_bt_node
- nav2_time_expired_condition_bt_node
- nav2_distance_traveled_condition_bt_node
- nav2_single_trigger_bt_node
- nav2_is_battery_low_condition_bt_node
- nav2_navigate_through_poses_action_bt_node
- nav2_navigate_to_pose_action_bt_node
- nav2_remove_passed_goals_action_bt_node
- nav2_planner_selector_bt_node
- nav2_controller_selector_bt_node
- nav2_goal_checker_selector_bt_node
bt_navigator_rclcpp_node:
ros__parameters:
use_sim_time: True
controller_server:
ros__parameters:
use_sim_time: True
controller_frequency: 20.0
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.5
min_theta_velocity_threshold: 0.001
progress_checker_plugin: "progress_checker"
goal_checker_plugin: "goal_checker"
controller_plugins: ["FollowPath"]
# Progress checker parameters
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.5
movement_time_allowance: 10.0
# Goal checker parameters
goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.25
yaw_goal_tolerance: 0.25
stateful: True
# DWB parameters
FollowPath:
plugin: "dwb_core::DWBLocalPlanner"
debug_trajectory_details: True
min_vel_x: 0.0
min_vel_y: 0.0
max_vel_x: 0.26
max_vel_y: 0.0
max_vel_theta: 1.0
min_speed_xy: 0.0
max_speed_xy: 0.26
min_speed_theta: 0.0
# Add high threshold velocity for turtlebot 3 issue.
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
acc_lim_x: 2.5
acc_lim_y: 0.0
acc_lim_theta: 3.2
decel_lim_x: -2.5
decel_lim_y: 0.0
decel_lim_theta: -3.2
vx_samples: 20
vy_samples: 5
vtheta_samples: 20
sim_time: 1.7
linear_granularity: 0.05
angular_granularity: 0.025
transform_tolerance: 0.2
trans_stopped_velocity: 0.25
short_circuit_trajectory_evaluation: True
stateful: True
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
BaseObstacle.scale: 0.02
PathAlign.scale: 0.0
GoalAlign.scale: 0.0
PathDist.scale: 32.0
GoalDist.scale: 24.0
RotateToGoal.scale: 32.0
RotateToGoal.slowing_factor: 5.0
RotateToGoal.lookahead_time: -1.0
controller_server_rclcpp_node:
ros__parameters:
use_sim_time: True
local_costmap:
local_costmap:
ros__parameters:
use_sim_time: True
global_frame: odom
rolling_window: true
width: 3
height: 3
resolution: 0.05
robot_radius: 0.22
plugins: ["obstacle_layer", "inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.55
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
scan:
topic: /robot1/scan
max_obstacle_height: 2.0
clearing: True
marking: True
raytrace_max_range: 3.0
raytrace_min_range: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
static_layer:
map_subscribe_transient_local: True
always_send_full_costmap: True
observation_sources: scan
local_costmap_client:
ros__parameters:
use_sim_time: True
local_costmap_rclcpp_node:
ros__parameters:
use_sim_time: True
global_costmap:
global_costmap:
ros__parameters:
use_sim_time: True
robot_radius: 0.22
obstacle_layer:
enabled: True
scan:
topic: /robot1/scan
max_obstacle_height: 2.0
clearing: True
marking: True
raytrace_max_range: 3.0
raytrace_min_range: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
static_layer:
map_subscribe_transient_local: True
always_send_full_costmap: True
observation_sources: scan
global_costmap_client:
ros__parameters:
use_sim_time: True
global_costmap_rclcpp_node:
ros__parameters:
use_sim_time: True
map_server:
ros__parameters:
use_sim_time: True
yaml_filename: "turtlebot3_world.yaml"
save_map_timeout: 5.0
planner_server:
ros__parameters:
use_sim_time: True
planner_plugins: ["GridBased"]
GridBased:
plugin: "nav2_navfn_planner/NavfnPlanner"
tolerance: 0.5
use_astar: false
allow_unknown: true
planner_server_rclcpp_node:
ros__parameters:
use_sim_time: True
recoveries_server:
ros__parameters:
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 10.0
recovery_plugins: ["spin", "backup", "wait"]
spin:
plugin: "nav2_recoveries/Spin"
backup:
plugin: "nav2_recoveries/BackUp"
wait:
plugin: "nav2_recoveries/Wait"
global_frame: odom
robot_base_frame: base_link
transform_timeout: 0.1
use_sim_time: true
simulate_ahead_time: 2.0
max_rotational_vel: 1.0
min_rotational_vel: 0.4
rotational_acc_lim: 3.2
robot_state_publisher:
ros__parameters:
use_sim_time: True
waypoint_follower:
ros__parameters:
loop_rate: 20
stop_on_failure: false
waypoint_task_executor_plugin: "wait_at_waypoint"
wait_at_waypoint:
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
enabled: True
waypoint_pause_duration: 200

287
map_merge/launch/tb3_simulation/config/nav2_multirobot_params_2.yaml Normal file
View File

@ -0,0 +1,287 @@
amcl:
ros__parameters:
use_sim_time: True
alpha1: 0.2
alpha2: 0.2
alpha3: 0.2
alpha4: 0.2
alpha5: 0.2
base_frame_id: "base_footprint"
beam_skip_distance: 0.5
beam_skip_error_threshold: 0.9
beam_skip_threshold: 0.3
do_beamskip: false
global_frame_id: "map"
lambda_short: 0.1
laser_likelihood_max_dist: 2.0
laser_max_range: 100.0
laser_min_range: -1.0
laser_model_type: "likelihood_field"
max_beams: 60
max_particles: 2000
min_particles: 500
odom_frame_id: "odom"
pf_err: 0.05
pf_z: 0.99
recovery_alpha_fast: 0.0
recovery_alpha_slow: 0.0
resample_interval: 1
robot_model_type: "differential"
save_pose_rate: 0.5
sigma_hit: 0.2
tf_broadcast: true
transform_tolerance: 1.0
update_min_a: 0.2
update_min_d: 0.25
z_hit: 0.5
z_max: 0.05
z_rand: 0.5
z_short: 0.05
scan_topic: scan
amcl_map_client:
ros__parameters:
use_sim_time: True
amcl_rclcpp_node:
ros__parameters:
use_sim_time: True
bt_navigator:
ros__parameters:
use_sim_time: True
global_frame: map
robot_base_frame: base_link
odom_topic: /odom
bt_loop_duration: 10
default_server_timeout: 20
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
# They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
# if enable_groot_monitoring is set to True, ports need to be different for each robot !!
enable_groot_monitoring: False
groot_zmq_publisher_port: 1789
groot_zmq_server_port: 1887
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
- nav2_compute_path_through_poses_action_bt_node
- nav2_follow_path_action_bt_node
- nav2_back_up_action_bt_node
- nav2_spin_action_bt_node
- nav2_wait_action_bt_node
- nav2_clear_costmap_service_bt_node
- nav2_is_stuck_condition_bt_node
- nav2_goal_reached_condition_bt_node
- nav2_goal_updated_condition_bt_node
- nav2_initial_pose_received_condition_bt_node
- nav2_reinitialize_global_localization_service_bt_node
- nav2_rate_controller_bt_node
- nav2_distance_controller_bt_node
- nav2_speed_controller_bt_node
- nav2_truncate_path_action_bt_node
- nav2_goal_updater_node_bt_node
- nav2_recovery_node_bt_node
- nav2_pipeline_sequence_bt_node
- nav2_round_robin_node_bt_node
- nav2_transform_available_condition_bt_node
- nav2_time_expired_condition_bt_node
- nav2_distance_traveled_condition_bt_node
- nav2_single_trigger_bt_node
- nav2_is_battery_low_condition_bt_node
- nav2_navigate_through_poses_action_bt_node
- nav2_navigate_to_pose_action_bt_node
- nav2_remove_passed_goals_action_bt_node
- nav2_planner_selector_bt_node
- nav2_controller_selector_bt_node
- nav2_goal_checker_selector_bt_node
bt_navigator_rclcpp_node:
ros__parameters:
use_sim_time: True
controller_server:
ros__parameters:
use_sim_time: True
controller_frequency: 20.0
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.5
min_theta_velocity_threshold: 0.001
progress_checker_plugin: "progress_checker"
goal_checker_plugin: "goal_checker"
controller_plugins: ["FollowPath"]
# Progress checker parameters
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.5
movement_time_allowance: 10.0
# Goal checker parameters
goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.25
yaw_goal_tolerance: 0.25
stateful: True
# DWB parameters
FollowPath:
plugin: "dwb_core::DWBLocalPlanner"
debug_trajectory_details: True
min_vel_x: 0.0
min_vel_y: 0.0
max_vel_x: 0.26
max_vel_y: 0.0
max_vel_theta: 1.0
min_speed_xy: 0.0
max_speed_xy: 0.26
min_speed_theta: 0.0
# Add high threshold velocity for turtlebot 3 issue.
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
acc_lim_x: 2.5
acc_lim_y: 0.0
acc_lim_theta: 3.2
decel_lim_x: -2.5
decel_lim_y: 0.0
decel_lim_theta: -3.2
vx_samples: 20
vy_samples: 5
vtheta_samples: 20
sim_time: 1.7
linear_granularity: 0.05
angular_granularity: 0.025
transform_tolerance: 0.2
trans_stopped_velocity: 0.25
short_circuit_trajectory_evaluation: True
stateful: True
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
BaseObstacle.scale: 0.02
PathAlign.scale: 0.0
GoalAlign.scale: 0.0
PathDist.scale: 32.0
GoalDist.scale: 24.0
RotateToGoal.scale: 32.0
RotateToGoal.slowing_factor: 5.0
RotateToGoal.lookahead_time: -1.0
controller_server_rclcpp_node:
ros__parameters:
use_sim_time: True
local_costmap:
local_costmap:
ros__parameters:
use_sim_time: True
global_frame: odom
rolling_window: true
width: 3
height: 3
resolution: 0.05
robot_radius: 0.22
plugins: ["obstacle_layer", "inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.55
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
scan:
topic: /robot2/scan
max_obstacle_height: 2.0
clearing: True
marking: True
raytrace_max_range: 3.0
raytrace_min_range: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
static_layer:
map_subscribe_transient_local: True
always_send_full_costmap: True
observation_sources: scan
local_costmap_client:
ros__parameters:
use_sim_time: True
local_costmap_rclcpp_node:
ros__parameters:
use_sim_time: True
global_costmap:
global_costmap:
ros__parameters:
use_sim_time: True
robot_radius: 0.22
obstacle_layer:
enabled: True
scan:
topic: /robot2/scan
max_obstacle_height: 2.0
clearing: True
marking: True
raytrace_max_range: 3.0
raytrace_min_range: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
static_layer:
map_subscribe_transient_local: True
always_send_full_costmap: True
observation_sources: scan
global_costmap_client:
ros__parameters:
use_sim_time: True
global_costmap_rclcpp_node:
ros__parameters:
use_sim_time: True
map_server:
ros__parameters:
use_sim_time: True
yaml_filename: "turtlebot3_world.yaml"
save_map_timeout: 5.0
planner_server:
ros__parameters:
use_sim_time: True
planner_plugins: ["GridBased"]
GridBased:
plugin: "nav2_navfn_planner/NavfnPlanner"
tolerance: 0.5
use_astar: false
allow_unknown: true
planner_server_rclcpp_node:
ros__parameters:
use_sim_time: True
recoveries_server:
ros__parameters:
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 10.0
recovery_plugins: ["spin", "backup", "wait"]
spin:
plugin: "nav2_recoveries/Spin"
backup:
plugin: "nav2_recoveries/BackUp"
wait:
plugin: "nav2_recoveries/Wait"
global_frame: odom
robot_base_frame: base_link
transform_timeout: 0.1
use_sim_time: true
simulate_ahead_time: 2.0
max_rotational_vel: 1.0
min_rotational_vel: 0.4
rotational_acc_lim: 3.2
robot_state_publisher:
ros__parameters:
use_sim_time: True
waypoint_follower:
ros__parameters:
loop_rate: 20
stop_on_failure: false
waypoint_task_executor_plugin: "wait_at_waypoint"
wait_at_waypoint:
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
enabled: True
waypoint_pause_duration: 200

293
map_merge/launch/tb3_simulation/multi_tb3_simulation_launch.py Normal file
View File

@ -0,0 +1,293 @@
# Copyright (c) 2018 Intel Corporation
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
"""
Example for spawing multiple robots in Gazebo.
This is an example on how to create a launch file for spawning multiple robots into Gazebo
and launch multiple instances of the navigation stack, each controlling one robot.
The robots co-exist on a shared environment and are controlled by independent nav stacks
"""
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription, condition
from launch.actions import (
DeclareLaunchArgument,
ExecuteProcess,
GroupAction,
IncludeLaunchDescription,
LogInfo,
)
from launch.conditions import IfCondition, UnlessCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, TextSubstitution
def generate_launch_description():
# Get the launch directory
bringup_dir = get_package_share_directory("nav2_bringup")
launch_dir = os.path.join(bringup_dir, "launch")
# Get the launch directory for multirobot_map_merge where we have a modified launch files
map_merge_dir = get_package_share_directory("multirobot_map_merge")
launch_dir_map_merge = os.path.join(map_merge_dir, "launch", "tb3_simulation")
# Names and poses of the robots for known poses demo
robots_known_poses = [
{"name": "robot1", "x_pose": 0.0, "y_pose": 0.5, "z_pose": 0.01},
{"name": "robot2", "x_pose": -3.0, "y_pose": 1.5, "z_pose": 0.01},
]
# Names and poses of the robots for unknown poses demo, the must be very close at beggining
robots_unknown_poses = [
{"name": "robot1", "x_pose": -2.0, "y_pose": 0.5, "z_pose": 0.01},
{"name": "robot2", "x_pose": -3.0, "y_pose": 0.5, "z_pose": 0.01},
]
# Simulation settings
world = LaunchConfiguration("world")
simulator = LaunchConfiguration("simulator")
# On this example all robots are launched with the same settings
map_yaml_file = LaunchConfiguration("map")
autostart = LaunchConfiguration("autostart")
rviz_config_file = LaunchConfiguration("rviz_config")
use_robot_state_pub = LaunchConfiguration("use_robot_state_pub")
use_rviz = LaunchConfiguration("use_rviz")
log_settings = LaunchConfiguration("log_settings", default="true")
known_init_poses = LaunchConfiguration("known_init_poses")
declare_known_init_poses_cmd = DeclareLaunchArgument(
"known_init_poses",
default_value="True",
description="Known initial poses of the robots. If so don't forget to declare them in the params.yaml file",
)
# Declare the launch arguments
declare_world_cmd = DeclareLaunchArgument(
"world",
default_value=os.path.join(launch_dir_map_merge, "worlds", "world_only.model"),
description="Full path to world file to load",
)
declare_simulator_cmd = DeclareLaunchArgument(
"simulator",
default_value="gazebo",
description="The simulator to use (gazebo or gzserver)",
)
declare_map_yaml_cmd = DeclareLaunchArgument(
"map",
default_value=os.path.join(bringup_dir, "maps", "turtlebot3_world.yaml"),
description="Full path to map file to load",
)
declare_robot1_params_file_cmd = DeclareLaunchArgument(
"robot1_params_file",
default_value=os.path.join(
launch_dir_map_merge, "config", "nav2_multirobot_params_1.yaml"
),
description="Full path to the ROS2 parameters file to use for robot1 launched nodes",
)
declare_robot2_params_file_cmd = DeclareLaunchArgument(
"robot2_params_file",
default_value=os.path.join(
launch_dir_map_merge, "config", "nav2_multirobot_params_2.yaml"
),
description="Full path to the ROS2 parameters file to use for robot2 launched nodes",
)
declare_autostart_cmd = DeclareLaunchArgument(
"autostart",
default_value="true",
description="Automatically startup the stacks",
)
declare_rviz_config_file_cmd = DeclareLaunchArgument(
"rviz_config",
default_value=os.path.join(bringup_dir, "rviz", "nav2_namespaced_view.rviz"),
description="Full path to the RVIZ config file to use.",
)
declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
"use_robot_state_pub",
default_value="True",
description="Whether to start the robot state publisher",
)
declare_use_rviz_cmd = DeclareLaunchArgument(
"use_rviz", default_value="True", description="Whether to start RVIZ"
)
slam_toolbox = LaunchConfiguration("slam_toolbox")
slam_gmapping = LaunchConfiguration("slam_gmapping")
declare_slam_toolbox_cmd = DeclareLaunchArgument(
"slam_toolbox", default_value="False", description="Whether run a SLAM toolbox"
)
declare_slam_gmapping_cmd = DeclareLaunchArgument(
"slam_gmapping",
default_value="False",
description="Whether run a SLAM gmapping",
)
# Start Gazebo with plugin providing the robot spawing service
start_gazebo_cmd = ExecuteProcess(
cmd=[
simulator,
"--verbose",
"-s",
"libgazebo_ros_init.so",
"-s",
"libgazebo_ros_factory.so",
world,
],
output="screen",
)
# Define commands for spawing the robots into Gazebo
spawn_robots_cmds = []
for robot_known, robot_unknown in zip(robots_known_poses, robots_unknown_poses):
spawn_robots_cmds.append(
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(bringup_dir, "launch", "spawn_tb3_launch.py")
),
launch_arguments={
"x_pose": TextSubstitution(text=str(robot_known["x_pose"])),
"y_pose": TextSubstitution(text=str(robot_known["y_pose"])),
"z_pose": TextSubstitution(text=str(robot_known["z_pose"])),
"robot_name": robot_known["name"],
"turtlebot_type": TextSubstitution(text="waffle"),
}.items(),
condition=IfCondition(known_init_poses),
)
)
spawn_robots_cmds.append(
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(bringup_dir, "launch", "spawn_tb3_launch.py")
),
launch_arguments={
"x_pose": TextSubstitution(text=str(robot_unknown["x_pose"])),
"y_pose": TextSubstitution(text=str(robot_unknown["y_pose"])),
"z_pose": TextSubstitution(text=str(robot_unknown["z_pose"])),
"robot_name": robot_unknown["name"],
"turtlebot_type": TextSubstitution(text="waffle"),
}.items(),
condition=UnlessCondition(known_init_poses),
)
)
# Define commands for launching the navigation instances
nav_instances_cmds = []
for robot in robots_known_poses:
params_file = LaunchConfiguration(f"{robot['name']}_params_file")
group = GroupAction(
[
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(launch_dir, "rviz_launch.py")
),
condition=IfCondition(use_rviz),
launch_arguments={
"namespace": TextSubstitution(text=robot["name"]),
"use_namespace": "True",
"rviz_config": rviz_config_file,
}.items(),
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(launch_dir_map_merge, "tb3_simulation_launch.py")
),
launch_arguments={
"namespace": robot["name"],
"use_namespace": "True",
"map": map_yaml_file,
"use_sim_time": "True",
"params_file": params_file,
"autostart": autostart,
"use_rviz": "False",
"use_simulator": "False",
"headless": "False",
"slam": "True",
"slam_toolbox": slam_toolbox,
"slam_gmapping": slam_gmapping,
"use_robot_state_pub": use_robot_state_pub,
}.items(),
),
LogInfo(
condition=IfCondition(log_settings),
msg=["Launching ", robot["name"]],
),
LogInfo(
condition=IfCondition(log_settings),
msg=[robot["name"], " map yaml: ", map_yaml_file],
),
LogInfo(
condition=IfCondition(log_settings),
msg=[robot["name"], " params yaml: ", params_file],
),
LogInfo(
condition=IfCondition(log_settings),
msg=[robot["name"], " rviz config file: ", rviz_config_file],
),
LogInfo(
condition=IfCondition(log_settings),
msg=[
robot["name"],
" using robot state pub: ",
use_robot_state_pub,
],
),
LogInfo(
condition=IfCondition(log_settings),
msg=[robot["name"], " autostart: ", autostart],
),
]
)
nav_instances_cmds.append(group)
# Create the launch description and populate
ld = LaunchDescription()
# Declare the launch options
ld.add_action(declare_simulator_cmd)
ld.add_action(declare_world_cmd)
ld.add_action(declare_map_yaml_cmd)
ld.add_action(declare_robot1_params_file_cmd)
ld.add_action(declare_robot2_params_file_cmd)
ld.add_action(declare_use_rviz_cmd)
ld.add_action(declare_autostart_cmd)
ld.add_action(declare_rviz_config_file_cmd)
ld.add_action(declare_use_robot_state_pub_cmd)
ld.add_action(declare_slam_toolbox_cmd)
ld.add_action(declare_slam_gmapping_cmd)
ld.add_action(declare_known_init_poses_cmd)
# Add the actions to start gazebo, robots and simulations
ld.add_action(start_gazebo_cmd)
for spawn_robot_cmd in spawn_robots_cmds:
ld.add_action(spawn_robot_cmd)
for simulation_instance_cmd in nav_instances_cmds:
ld.add_action(simulation_instance_cmd)
return ld

49
map_merge/launch/tb3_simulation/slam_toolbox.py Normal file
View File

@ -0,0 +1,49 @@
import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
use_sim_time = LaunchConfiguration("use_sim_time")
slam_params_file = LaunchConfiguration("slam_params_file")
remappings = [
("/map", "map"),
("/scan", "scan"),
("/tf", "tf"),
("/tf_static", "tf_static"),
]
declare_use_sim_time_argument = DeclareLaunchArgument(
"use_sim_time", default_value="true", description="Use simulation/Gazebo clock"
)
declare_slam_params_file_cmd = DeclareLaunchArgument(
"slam_params_file",
default_value=os.path.join(
get_package_share_directory("slam_toolbox"),
"config",
"mapper_params_online_sync.yaml",
),
description="Full path to the ROS2 parameters file to use for the slam_toolbox node",
)
start_sync_slam_toolbox_node = Node(
parameters=[slam_params_file, {"use_sim_time": use_sim_time}],
package="slam_toolbox",
executable="sync_slam_toolbox_node",
name="slam_toolbox",
output="screen",
remappings=remappings,
)
ld = LaunchDescription()
ld.add_action(declare_use_sim_time_argument)
ld.add_action(declare_slam_params_file_cmd)
ld.add_action(start_sync_slam_toolbox_node)
return ld

245
map_merge/launch/tb3_simulation/tb3_simulation_launch.py Normal file
View File

@ -0,0 +1,245 @@
# Copyright (c) 2018 Intel Corporation
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
"""This is all-in-one launch script intended for use by nav2 developers."""
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
ExecuteProcess,
IncludeLaunchDescription,
)
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch_ros.actions import Node
def generate_launch_description():
# Get the launch directory
bringup_dir = get_package_share_directory("nav2_bringup")
launch_dir = os.path.join(bringup_dir, "launch")
# Get the launch directory for multirobot_map_merge where we have a modified bringup launch file
map_merge_dir = get_package_share_directory("multirobot_map_merge")
launch_dir_map_merge = os.path.join(map_merge_dir, "launch", "tb3_simulation")
# Create the launch configuration variables
slam = LaunchConfiguration("slam")
slam_toolbox = LaunchConfiguration("slam_toolbox")
slam_gmapping = LaunchConfiguration("slam_gmapping")
namespace = LaunchConfiguration("namespace")
use_namespace = LaunchConfiguration("use_namespace")
map_yaml_file = LaunchConfiguration("map")
use_sim_time = LaunchConfiguration("use_sim_time")
params_file = LaunchConfiguration("params_file")
autostart = LaunchConfiguration("autostart")
# Launch configuration variables specific to simulation
rviz_config_file = LaunchConfiguration("rviz_config_file")
use_simulator = LaunchConfiguration("use_simulator")
use_robot_state_pub = LaunchConfiguration("use_robot_state_pub")
use_rviz = LaunchConfiguration("use_rviz")
headless = LaunchConfiguration("headless")
world = LaunchConfiguration("world")
# Map fully qualified names to relative ones so the node's namespace can be prepended.
# In case of the transforms (tf), currently, there doesn't seem to be a better alternative
# https://github.com/ros/geometry2/issues/32
# https://github.com/ros/robot_state_publisher/pull/30
# TODO(orduno) Substitute with `PushNodeRemapping`
# https://github.com/ros2/launch_ros/issues/56
remappings = [("/tf", "tf"), ("/tf_static", "tf_static")]
# Declare the launch arguments
declare_namespace_cmd = DeclareLaunchArgument(
"namespace", default_value="", description="Top-level namespace"
)
declare_use_namespace_cmd = DeclareLaunchArgument(
"use_namespace",
default_value="false",
description="Whether to apply a namespace to the navigation stack",
)
declare_slam_cmd = DeclareLaunchArgument(
"slam", default_value="False", description="Whether run a SLAM"
)
declare_slam_toolbox_cmd = DeclareLaunchArgument(
"slam_toolbox", default_value="False", description="Whether run a SLAM toolbox"
)
declare_slam_gmapping_cmd = DeclareLaunchArgument(
"slam_gmapping",
default_value="False",
description="Whether run a SLAM gmapping",
)
declare_map_yaml_cmd = DeclareLaunchArgument(
"map",
default_value=os.path.join(bringup_dir, "maps", "turtlebot3_world.yaml"),
description="Full path to map file to load",
)
declare_use_sim_time_cmd = DeclareLaunchArgument(
"use_sim_time",
default_value="true",
description="Use simulation (Gazebo) clock if true",
)
declare_params_file_cmd = DeclareLaunchArgument(
"params_file",
default_value=os.path.join(bringup_dir, "params", "nav2_params.yaml"),
description="Full path to the ROS2 parameters file to use for all launched nodes",
)
declare_autostart_cmd = DeclareLaunchArgument(
"autostart",
default_value="true",
description="Automatically startup the nav2 stack",
)
declare_rviz_config_file_cmd = DeclareLaunchArgument(
"rviz_config_file",
default_value=os.path.join(bringup_dir, "rviz", "nav2_default_view.rviz"),
description="Full path to the RVIZ config file to use",
)
declare_use_simulator_cmd = DeclareLaunchArgument(
"use_simulator",
default_value="True",
description="Whether to start the simulator",
)
declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
"use_robot_state_pub",
default_value="True",
description="Whether to start the robot state publisher",
)
declare_use_rviz_cmd = DeclareLaunchArgument(
"use_rviz", default_value="True", description="Whether to start RVIZ"
)
declare_simulator_cmd = DeclareLaunchArgument(
"headless", default_value="False", description="Whether to execute gzclient)"
)
declare_world_cmd = DeclareLaunchArgument(
"world",
# TODO(orduno) Switch back once ROS argument passing has been fixed upstream
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/91
# default_value=os.path.join(get_package_share_directory('turtlebot3_gazebo'),
# worlds/turtlebot3_worlds/waffle.model')
default_value=os.path.join(bringup_dir, "worlds", "waffle.model"),
description="Full path to world model file to load",
)
# Specify the actions
start_gazebo_server_cmd = ExecuteProcess(
condition=IfCondition(use_simulator),
cmd=[
"gzserver",
"-s",
"libgazebo_ros_init.so",
"-s",
"libgazebo_ros_factory.so",
world,
],
cwd=[launch_dir],
output="screen",
)
start_gazebo_client_cmd = ExecuteProcess(
condition=IfCondition(PythonExpression([use_simulator, " and not ", headless])),
cmd=["gzclient"],
cwd=[launch_dir],
output="screen",
)
urdf = os.path.join(bringup_dir, "urdf", "turtlebot3_waffle.urdf")
start_robot_state_publisher_cmd = Node(
condition=IfCondition(use_robot_state_pub),
package="robot_state_publisher",
executable="robot_state_publisher",
name="robot_state_publisher",
namespace=namespace,
output="screen",
parameters=[{"use_sim_time": use_sim_time}],
remappings=remappings,
arguments=[urdf],
)
rviz_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(launch_dir, "rviz_launch.py")),
condition=IfCondition(use_rviz),
launch_arguments={
"namespace": "",
"use_namespace": "False",
"rviz_config": rviz_config_file,
}.items(),
)
bringup_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(launch_dir_map_merge, "bringup_launch.py")
),
launch_arguments={
"namespace": namespace,
"use_namespace": use_namespace,
"slam": slam,
"slam_toolbox": slam_toolbox,
"slam_gmapping": slam_gmapping,
"map": map_yaml_file,
"use_sim_time": use_sim_time,
"params_file": params_file,
"autostart": autostart,
}.items(),
)
# Create the launch description and populate
ld = LaunchDescription()
# Declare the launch options
ld.add_action(declare_namespace_cmd)
ld.add_action(declare_use_namespace_cmd)
ld.add_action(declare_slam_cmd)
ld.add_action(declare_slam_toolbox_cmd)
ld.add_action(declare_slam_gmapping_cmd)
ld.add_action(declare_map_yaml_cmd)
ld.add_action(declare_use_sim_time_cmd)
ld.add_action(declare_params_file_cmd)
ld.add_action(declare_autostart_cmd)
ld.add_action(declare_rviz_config_file_cmd)
ld.add_action(declare_use_simulator_cmd)
ld.add_action(declare_use_robot_state_pub_cmd)
ld.add_action(declare_use_rviz_cmd)
ld.add_action(declare_simulator_cmd)
ld.add_action(declare_world_cmd)
# Add any conditioned actions
ld.add_action(start_gazebo_server_cmd)
ld.add_action(start_gazebo_client_cmd)
# Add the actions to launch all of the navigation nodes
ld.add_action(start_robot_state_publisher_cmd)
ld.add_action(rviz_cmd)
ld.add_action(bringup_cmd)
return ld

56
map_merge/launch/tb3_simulation/worlds/world_only.model Normal file
View File

@ -0,0 +1,56 @@
<?xml version="1.0"?>
<sdf version="1.6">
<world name="default">
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://sun</uri>
</include>
<scene>
<shadows>false</shadows>
</scene>
<gui fullscreen='0'>
<camera name='user_camera'>
<pose frame=''>0.319654 -0.235002 9.29441 0 1.5138 0.009599</pose>
<view_controller>orbit</view_controller>
<projection_type>perspective</projection_type>
</camera>
</gui>
<physics type="ode">
<real_time_update_rate>1000.0</real_time_update_rate>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<ode>
<solver>
<type>quick</type>
<iters>150</iters>
<precon_iters>0</precon_iters>
<sor>1.400000</sor>
<use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
</solver>
<constraints>
<cfm>0.00001</cfm>
<erp>0.2</erp>
<contact_max_correcting_vel>2000.000000</contact_max_correcting_vel>
<contact_surface_layer>0.01000</contact_surface_layer>
</constraints>
</ode>
</physics>
<model name="turtlebot3_world">
<static>1</static>
<include>
<!-- <uri>model://turtlebot3_dqn_world</uri> -->
<!-- <uri>model://turtlebot3_world</uri> -->
<uri>model://turtlebot3_house</uri>
</include>
</model>
</world>
</sdf>

29
map_merge/package.xml Normal file
View File

@ -0,0 +1,29 @@
<?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>multirobot_map_merge</name>
<version>1.0.0</version>
<description>Merging multiple maps without knowledge of initial
positions of robots 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>ament_cmake</depend>
<depend>geometry_msgs</depend>
<depend>nav_msgs</depend>
<depend>map_msgs</depend>
<depend>tf2_geometry_msgs</depend>
<!-- used to get OpenCV -->
<depend>image_geometry</depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

152
map_merge/src/combine_grids/estimation_internal.h Normal file
View File

@ -0,0 +1,152 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2015-2016, Jiri Horner.
* Copyright (c) 2021, 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 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 ESTIMATION_INTERNAL_H_
#define ESTIMATION_INTERNAL_H_
#include <combine_grids/merging_pipeline.h>
#include <iostream>
#include <cassert>
#include <opencv2/core/utility.hpp>
#include <opencv2/core/version.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/stitching/detail/matchers.hpp>
#ifdef HAVE_OPENCV_XFEATURES2D
#include <opencv2/xfeatures2d/nonfree.hpp>
#endif
namespace combine_grids
{
namespace internal
{
#if CV_VERSION_MAJOR >= 4
static inline cv::Ptr<cv::Feature2D> chooseFeatureFinder(FeatureType type)
{
switch (type) {
case FeatureType::AKAZE:
return cv::AKAZE::create();
case FeatureType::ORB:
return cv::ORB::create();
case FeatureType::SURF:
#ifdef HAVE_OPENCV_XFEATURES2D
return cv::xfeatures2d::SURF::create();
#else
return cv::AKAZE::create();
#endif
}
assert(false);
return {};
}
#else // (CV_VERSION_MAJOR < 4)
static inline cv::Ptr<cv::detail::FeaturesFinder>
chooseFeatureFinder(FeatureType type)
{
switch (type) {
case FeatureType::AKAZE:
return cv::makePtr<cv::detail::AKAZEFeaturesFinder>();
case FeatureType::ORB:
return cv::makePtr<cv::detail::OrbFeaturesFinder>();
case FeatureType::SURF:
return cv::makePtr<cv::detail::SurfFeaturesFinder>();
}
assert(false);
return {};
}
#endif // CV_VERSION_MAJOR >= 4
static inline void writeDebugMatchingInfo(
const std::vector<cv::Mat>& images,
const std::vector<cv::detail::ImageFeatures>& image_features,
const std::vector<cv::detail::MatchesInfo>& pairwise_matches)
{
for (auto& match_info : pairwise_matches) {
if (match_info.H.empty() ||
match_info.src_img_idx >= match_info.dst_img_idx) {
continue;
}
std::cout << match_info.src_img_idx << " " << match_info.dst_img_idx
<< std::endl
<< "features: "
<< image_features[size_t(match_info.src_img_idx)].keypoints.size()
<< " "
<< image_features[size_t(match_info.dst_img_idx)].keypoints.size()
<< std::endl
<< "matches: " << match_info.matches.size() << std::endl
<< "inliers: " << match_info.num_inliers << std::endl
<< "inliers/matches ratio: "
<< match_info.num_inliers / double(match_info.matches.size())
<< std::endl
<< "confidence: " << match_info.confidence << std::endl
<< match_info.H << std::endl;
cv::Mat img;
// draw all matches
cv::drawMatches(images[size_t(match_info.src_img_idx)],
image_features[size_t(match_info.src_img_idx)].keypoints,
images[size_t(match_info.dst_img_idx)],
image_features[size_t(match_info.dst_img_idx)].keypoints,
match_info.matches, img);
cv::imwrite(std::to_string(match_info.src_img_idx) + "_" +
std::to_string(match_info.dst_img_idx) + "_matches.png",
img);
// draw inliers only
cv::drawMatches(
images[size_t(match_info.src_img_idx)],
image_features[size_t(match_info.src_img_idx)].keypoints,
images[size_t(match_info.dst_img_idx)],
image_features[size_t(match_info.dst_img_idx)].keypoints,
match_info.matches, img, cv::Scalar::all(-1), cv::Scalar::all(-1),
*reinterpret_cast<const std::vector<char>*>(&match_info.inliers_mask));
cv::imwrite(std::to_string(match_info.src_img_idx) + "_" +
std::to_string(match_info.dst_img_idx) +
"_matches_inliers.png",
img);
}
}
} // namespace internal
} // namespace combine_grids
#endif // ESTIMATION_INTERNAL_H_

88
map_merge/src/combine_grids/grid_compositor.cpp Normal file
View File

@ -0,0 +1,88 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2015-2016, Jiri Horner.
* Copyright (c) 2021, 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 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 <combine_grids/grid_compositor.h>
#include <opencv2/stitching/detail/util.hpp>
#include <rcpputils/asserts.hpp>
namespace combine_grids
{
namespace internal
{
nav_msgs::msg::OccupancyGrid::SharedPtr GridCompositor::compose(
const std::vector<cv::Mat>& grids, const std::vector<cv::Rect>& rois)
{
rcpputils::require_true(grids.size() == rois.size());
nav_msgs::msg::OccupancyGrid::SharedPtr result_grid(new nav_msgs::msg::OccupancyGrid());
std::vector<cv::Point> corners;
corners.reserve(grids.size());
std::vector<cv::Size> sizes;
sizes.reserve(grids.size());
for (auto& roi : rois) {
corners.push_back(roi.tl());
sizes.push_back(roi.size());
}
cv::Rect dst_roi = cv::detail::resultRoi(corners, sizes);
result_grid->info.width = static_cast<uint>(dst_roi.width);
result_grid->info.height = static_cast<uint>(dst_roi.height);
result_grid->data.resize(static_cast<size_t>(dst_roi.area()), -1);
// create view for opencv pointing to newly allocated grid
cv::Mat result(dst_roi.size(), CV_8S, result_grid->data.data());
for (size_t i = 0; i < grids.size(); ++i) {
// we need to compensate global offset
cv::Rect roi = cv::Rect(corners[i] - dst_roi.tl(), sizes[i]);
cv::Mat result_roi(result, roi);
// reinterpret warped matrix as signed
// we will not change this matrix, but opencv does not support const matrices
cv::Mat warped_signed (grids[i].size(), CV_8S, const_cast<uchar*>(grids[i].ptr()));
// compose img into result matrix
cv::max(result_roi, warped_signed, result_roi);
}
return result_grid;
}
} // namespace internal
} // namespace combine_grids

90
map_merge/src/combine_grids/grid_warper.cpp Normal file
View File

@ -0,0 +1,90 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2015-2016, Jiri Horner.
* Copyright (c) 2021, 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 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 <combine_grids/grid_warper.h>
#include <opencv2/stitching/detail/warpers.hpp>
#include <rcpputils/asserts.hpp>
namespace combine_grids
{
namespace internal
{
cv::Rect GridWarper::warp(const cv::Mat& grid, const cv::Mat& transform,
cv::Mat& warped_grid)
{
// for validating function inputs. Throws an std::invalid_argument exception if the condition fails.
rcpputils::require_true(transform.type() == CV_64F);
cv::Mat H;
invertAffineTransform(transform.rowRange(0, 2), H);
cv::Rect roi = warpRoi(grid, H);
// shift top left corner for warp affine (otherwise the image is cropped)
H.at<double>(0, 2) -= roi.tl().x;
H.at<double>(1, 2) -= roi.tl().y;
warpAffine(grid, warped_grid, H, roi.size(), cv::INTER_NEAREST,
cv::BORDER_CONSTANT,
cv::Scalar::all(255) /* this is -1 for signed char */);
// For verifying results. Throws a rcpputils::AssertionException if the condition fails.
// This function becomes a no-op in release builds.
rcpputils::assert_true(roi.size() == warped_grid.size());
return roi;
}
cv::Rect GridWarper::warpRoi(const cv::Mat& grid, const cv::Mat& transform)
{
cv::Ptr<cv::detail::PlaneWarper> warper =
cv::makePtr<cv::detail::PlaneWarper>();
cv::Mat H;
transform.convertTo(H, CV_32F);
// separate rotation and translation for plane warper
// 3D translation
cv::Mat T = cv::Mat::zeros(3, 1, CV_32F);
H.colRange(2, 3).rowRange(0, 2).copyTo(T.rowRange(0, 2));
// 3D rotation
cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
H.colRange(0, 2).copyTo(R.rowRange(0, 2).colRange(0, 2));
return warper->warpRoi(grid.size(), cv::Mat::eye(3, 3, CV_32F), R, T);
}
} // namespace internal
} // namespace combine_grids

280
map_merge/src/combine_grids/merging_pipeline.cpp Normal file
View File

@ -0,0 +1,280 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2015-2016, Jiri Horner.
* Copyright (c) 2021, 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 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 <combine_grids/grid_compositor.h>
#include <combine_grids/grid_warper.h>
#include <combine_grids/merging_pipeline.h>
#include <rclcpp/rclcpp.hpp>
#include <rcpputils/asserts.hpp>
#include <opencv2/stitching/detail/matchers.hpp>
#include <opencv2/stitching/detail/motion_estimators.hpp>
#include "estimation_internal.h"
namespace combine_grids
{
bool MergingPipeline::estimateTransforms(FeatureType feature_type,
double confidence)
{
std::vector<cv::detail::ImageFeatures> image_features;
std::vector<cv::detail::MatchesInfo> pairwise_matches;
std::vector<cv::detail::CameraParams> transforms;
std::vector<int> good_indices;
// TODO investigate value translation effect on features
auto finder = internal::chooseFeatureFinder(feature_type);
cv::Ptr<cv::detail::FeaturesMatcher> matcher =
cv::makePtr<cv::detail::AffineBestOf2NearestMatcher>();
cv::Ptr<cv::detail::Estimator> estimator =
cv::makePtr<cv::detail::AffineBasedEstimator>();
cv::Ptr<cv::detail::BundleAdjusterBase> adjuster =
cv::makePtr<cv::detail::BundleAdjusterAffinePartial>();
if (images_.empty()) {
return true;
}
/* find features in images */
static rclcpp::Logger logger = rclcpp::get_logger("estimateTransforms");
RCLCPP_DEBUG(logger, "computing features");
image_features.reserve(images_.size());
for (const cv::Mat& image : images_) {
image_features.emplace_back();
if (!image.empty()) {
#if CV_VERSION_MAJOR >= 4
cv::detail::computeImageFeatures(finder, image, image_features.back());
#else
(*finder)(image, image_features.back());
#endif
}
}
finder = {};
/* find corespondent features */
RCLCPP_DEBUG(logger, "pairwise matching features");
(*matcher)(image_features, pairwise_matches);
matcher = {};
#ifndef NDEBUG
internal::writeDebugMatchingInfo(images_, image_features, pairwise_matches);
#endif
/* use only matches that has enough confidence. leave out matches that are not
* connected (small components) */
good_indices = cv::detail::leaveBiggestComponent(
image_features, pairwise_matches, static_cast<float>(confidence));
// no match found. try set first non-empty grid as reference frame. we try to
// avoid setting empty grid as reference frame, in case some maps never
// arrive. If all is empty just set null transforms.
// if (good_indices.size() == 1) {
// transforms_.clear();
// transforms_.resize(images_.size());
// for (size_t i = 0; i < images_.size(); ++i) {
// if (!images_[i].empty()) {
// // set identity
// transforms_[i] = cv::Mat::eye(3, 3, CV_64F);
// break;
// }
// }
// // RCLCPP_INFO(logger, "No match found between maps, setting first non-empty grid as reference frame");
// return true;
// }
// Making some tests it is better to just return false if no match is found
// and not clear the last good transforms found
if (good_indices.size() == 1) {
if (images_.size() != transforms_.size()) {
transforms_.clear();
transforms_.resize(images_.size());
}
return false;
}
// // Experimental: should we keep only the best confidence match overall?
// bool max_confidence_achieved_surpassed = false;
// for (auto &match_info : pairwise_matches) {
// RCLCPP_INFO(logger, "match info: %f", match_info.confidence);
// if (match_info.confidence > max_confidence_achieved_){
// max_confidence_achieved_surpassed = true;
// max_confidence_achieved_ = match_info.confidence;
// }
// }
// if (!max_confidence_achieved_surpassed) {
// RCLCPP_INFO(logger, "Max confidence achieved not surpassed, not using matching");
// return false;
// }
// else
// RCLCPP_INFO(logger, "Max confidence achieved surpassed, optimizing");
/* estimate transform */
RCLCPP_DEBUG(logger, "calculating transforms in global reference frame");
// note: currently used estimator never fails
if (!(*estimator)(image_features, pairwise_matches, transforms)) {
return false;
}
/* levmarq optimization */
// openCV just accepts float transforms
for (auto& transform : transforms) {
transform.R.convertTo(transform.R, CV_32F);
}
RCLCPP_DEBUG(logger, "optimizing global transforms");
adjuster->setConfThresh(confidence);
if (!(*adjuster)(image_features, pairwise_matches, transforms)) {
RCLCPP_WARN(logger, "Bundle adjusting failed. Could not estimate transforms.");
return false;
}
transforms_.clear();
transforms_.resize(images_.size());
size_t i = 0;
for (auto& j : good_indices) {
// we want to work with transforms as doubles
transforms[i].R.convertTo(transforms_[static_cast<size_t>(j)], CV_64F);
++i;
}
return true;
}
// checks whether given matrix is an identity, i.e. exactly appropriate Mat::eye
static inline bool isIdentity(const cv::Mat& matrix)
{
if (matrix.empty()) {
return false;
}
cv::MatExpr diff = matrix != cv::Mat::eye(matrix.size(), matrix.type());
return cv::countNonZero(diff) == 0;
}
nav_msgs::msg::OccupancyGrid::SharedPtr MergingPipeline::composeGrids()
{
// for checking states. Throws a rcpputils::IllegalStateException if the condition fails.
rcpputils::check_true(images_.size() == transforms_.size());
rcpputils::check_true(images_.size() == grids_.size());
static rclcpp::Logger logger = rclcpp::get_logger("composeGrids");
if (images_.empty()) {
return nullptr;
}
RCLCPP_DEBUG(logger, "warping grids");
internal::GridWarper warper;
std::vector<cv::Mat> imgs_warped;
imgs_warped.reserve(images_.size());
std::vector<cv::Rect> rois;
rois.reserve(images_.size());
for (size_t i = 0; i < images_.size(); ++i) {
if (!transforms_[i].empty() && !images_[i].empty()) {
imgs_warped.emplace_back();
rois.emplace_back(
warper.warp(images_[i], transforms_[i], imgs_warped.back()));
}
}
if (imgs_warped.empty()) {
return nullptr;
}
RCLCPP_DEBUG(logger, "compositing result grid");
nav_msgs::msg::OccupancyGrid::SharedPtr result;
internal::GridCompositor compositor;
result = compositor.compose(imgs_warped, rois);
// set correct resolution to output grid. use resolution of identity (works
// for estimated transforms), or any resolution (works for know_init_positions)
// - in that case all resolutions should be the same.
float any_resolution = 0.0;
for (size_t i = 0; i < transforms_.size(); ++i) {
// check if this transform is the reference frame
if (isIdentity(transforms_[i])) {
result->info.resolution = grids_[i]->info.resolution;
break;
}
if (grids_[i]) {
any_resolution = grids_[i]->info.resolution;
}
}
if (result->info.resolution <= 0.f) {
result->info.resolution = any_resolution;
}
// set grid origin to its centre
result->info.origin.position.x =
-(result->info.width / 2.0) * double(result->info.resolution);
result->info.origin.position.y =
-(result->info.height / 2.0) * double(result->info.resolution);
result->info.origin.orientation.w = 1.0;
return result;
}
std::vector<geometry_msgs::msg::Transform> MergingPipeline::getTransforms() const
{
std::vector<geometry_msgs::msg::Transform> result;
result.reserve(transforms_.size());
for (auto& transform : transforms_) {
if (transform.empty()) {
result.emplace_back();
continue;
}
rcpputils::require_true(transform.type() == CV_64F);
geometry_msgs::msg::Transform ros_transform;
ros_transform.translation.x = transform.at<double>(0, 2);
ros_transform.translation.y = transform.at<double>(1, 2);
ros_transform.translation.z = 0.;
// our rotation is in fact only 2D, thus quaternion can be simplified
double a = transform.at<double>(0, 0);
double b = transform.at<double>(1, 0);
ros_transform.rotation.w = std::sqrt(2. + 2. * a) * 0.5;
ros_transform.rotation.x = 0.;
ros_transform.rotation.y = 0.;
ros_transform.rotation.z = std::copysign(std::sqrt(2. - 2. * a) * 0.5, b);
result.push_back(ros_transform);
}
return result;
}
} // namespace combine_grids

454
map_merge/src/map_merge.cpp Normal file
View File

@ -0,0 +1,454 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2014, Zhi Yan.
* Copyright (c) 2015-2016, Jiri Horner.
* Copyright (c) 2021, 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 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 <thread>
#include <map_merge/map_merge.h>
#include <map_merge/ros1_names.hpp>
#include <rcpputils/asserts.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
namespace map_merge
{
MapMerge::MapMerge() : Node("map_merge", rclcpp::NodeOptions()
.allow_undeclared_parameters(true)
.automatically_declare_parameters_from_overrides(true)),
subscriptions_size_(0)
{
std::string frame_id;
std::string merged_map_topic;
if (!this->has_parameter("merging_rate")) this->declare_parameter<double>("merging_rate", 4.0);
if (!this->has_parameter("discovery_rate")) this->declare_parameter<double>("discovery_rate", 0.05);
if (!this->has_parameter("estimation_rate")) this->declare_parameter<double>("estimation_rate", 0.5);
if (!this->has_parameter("known_init_poses")) this->declare_parameter<bool>("known_init_poses", true);
if (!this->has_parameter("estimation_confidence")) this->declare_parameter<double>("estimation_confidence", 1.0);
if (!this->has_parameter("robot_map_topic")) this->declare_parameter<std::string>("robot_map_topic", "map");
if (!this->has_parameter("robot_map_updates_topic")) this->declare_parameter<std::string>("robot_map_updates_topic", "map_updates");
if (!this->has_parameter("robot_namespace")) this->declare_parameter<std::string>("robot_namespace", "");
if (!this->has_parameter("merged_map_topic")) this->declare_parameter<std::string>("merged_map_topic", "map");
if (!this->has_parameter("world_frame")) this->declare_parameter<std::string>("world_frame", "world");
this->get_parameter("merging_rate", merging_rate_);
this->get_parameter("discovery_rate", discovery_rate_);
this->get_parameter("estimation_rate", estimation_rate_);
this->get_parameter("known_init_poses", have_initial_poses_);
this->get_parameter("estimation_confidence", confidence_threshold_);
this->get_parameter("robot_map_topic", robot_map_topic_);
this->get_parameter("robot_map_updates_topic", robot_map_updates_topic_);
this->get_parameter("robot_namespace", robot_namespace_);
this->get_parameter("merged_map_topic", merged_map_topic);
this->get_parameter("world_frame", world_frame_);
/* publishing */
// Create a publisher using the QoS settings to emulate a ROS1 latched topic
merged_map_publisher_ =
this->create_publisher<nav_msgs::msg::OccupancyGrid>(merged_map_topic,
rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
// Timers
map_merging_timer_ = this->create_wall_timer(
std::chrono::milliseconds((uint16_t)(1000.0 / merging_rate_)),
[this]() { mapMerging(); });
// execute right away to simulate the ros1 first while loop on a thread
map_merging_timer_->execute_callback();
topic_subscribing_timer_ = this->create_wall_timer(
std::chrono::milliseconds((uint16_t)(1000.0 / discovery_rate_)),
[this]() { topicSubscribing(); });
// For topicSubscribing() we need to spin briefly for the discovery to happen
rclcpp::Rate r(100);
int i = 0;
while (rclcpp::ok() && i < 100) {
rclcpp::spin_some(this->get_node_base_interface());
r.sleep();
i++;
}
topic_subscribing_timer_->execute_callback();
if (!have_initial_poses_){
pose_estimation_timer_ = this->create_wall_timer(
std::chrono::milliseconds((uint16_t)(1000.0 / estimation_rate_)),
[this]() { poseEstimation(); });
// execute right away to simulate the ros1 first while loop on a thread
pose_estimation_timer_->execute_callback();
}
}
/*
* Subcribe to pose and map topics
*/
void MapMerge::topicSubscribing()
{
RCLCPP_DEBUG(logger_, "Robot discovery started.");
RCLCPP_INFO_ONCE(logger_, "Robot discovery started.");
// ros::master::V_TopicInfo topic_infos;
geometry_msgs::msg::Transform init_pose;
std::string robot_name;
std::string map_topic;
std::string map_updates_topic;
// ros::master::getTopics(topic_infos);
std::map<std::string, std::vector<std::string>> topic_infos = this->get_topic_names_and_types();
for (const auto& topic_it : topic_infos) {
std::string topic_name = topic_it.first;
std::vector<std::string> topic_types = topic_it.second;
// iterate over all topic types
for (const auto& topic_type : topic_types) {
// RCLCPP_INFO(logger_, "Topic: %s, type: %s", topic_name.c_str(), topic_type.c_str());
// we check only map topic
if (!isRobotMapTopic(topic_name, topic_type)) {
continue;
}
robot_name = robotNameFromTopic(topic_name);
if (robots_.count(robot_name)) {
// we already know this robot
continue;
}
if (have_initial_poses_ && !getInitPose(robot_name, init_pose)) {
RCLCPP_WARN(logger_, "Couldn't get initial position for robot [%s]\n"
"did you defined parameters map_merge/init_pose_[xyz]? in robot "
"namespace? If you want to run merging without known initial "
"positions of robots please set `known_init_poses` parameter "
"to false. See relevant documentation for details.",
robot_name.c_str());
continue;
}
RCLCPP_INFO(logger_, "adding robot [%s] to system", robot_name.c_str());
{
// We don't lock since because of ROS2 default executor only a callback can run at a time
// std::lock_guard<boost::shared_mutex> lock(subscriptions_mutex_);
subscriptions_.emplace_front();
++subscriptions_size_;
}
// no locking here. robots_ are used only in this procedure
MapSubscription& subscription = subscriptions_.front();
robots_.insert({robot_name, &subscription});
subscription.initial_pose = init_pose;
/* subscribe callbacks */
map_topic = ros1_names::append(robot_name, robot_map_topic_);
map_updates_topic =
ros1_names::append(robot_name, robot_map_updates_topic_);
RCLCPP_INFO(logger_, "Subscribing to MAP topic: %s.", map_topic.c_str());
auto map_qos = rclcpp::QoS(rclcpp::KeepLast(50)).transient_local().reliable();
subscription.map_sub = this->create_subscription<nav_msgs::msg::OccupancyGrid>(
map_topic, map_qos,
[this, &subscription](const nav_msgs::msg::OccupancyGrid::SharedPtr msg) {
fullMapUpdate(msg, subscription);
});
RCLCPP_INFO(logger_, "Subscribing to MAP updates topic: %s.",
map_updates_topic.c_str());
subscription.map_updates_sub =
this->create_subscription<map_msgs::msg::OccupancyGridUpdate>(
map_updates_topic, map_qos,
[this, &subscription](
const map_msgs::msg::OccupancyGridUpdate::SharedPtr msg) {
partialMapUpdate(msg, subscription);
});
}
}
}
/*
* mapMerging()
*/
void MapMerge::mapMerging()
{
RCLCPP_DEBUG(logger_, "Map merging started.");
RCLCPP_INFO_ONCE(logger_, "Map merging started.");
if (have_initial_poses_) {
// TODO: attempt fix for SLAM toolbox: add method for padding grids to same size
std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> grids;
std::vector<geometry_msgs::msg::Transform> transforms;
grids.reserve(subscriptions_size_);
{
// We don't lock since because of ROS2 default executor only a callback can run
// boost::shared_lock<boost::shared_mutex> lock(subscriptions_mutex_);
for (auto& subscription : subscriptions_) {
// std::lock_guard<std::mutex> s_lock(subscription.mutex);
grids.push_back(subscription.readonly_map);
transforms.push_back(subscription.initial_pose);
}
}
// we don't need to lock here, because when have_initial_poses_ is true we
// will not run concurrently on the pipeline
pipeline_.feed(grids.begin(), grids.end());
pipeline_.setTransforms(transforms.begin(), transforms.end());
}
// nav_msgs::OccupancyGridPtr merged_map;
nav_msgs::msg::OccupancyGrid::SharedPtr merged_map;
{
std::lock_guard<std::mutex> lock(pipeline_mutex_);
merged_map = pipeline_.composeGrids();
}
if (!merged_map) {
// RCLCPP_INFO(logger_, "No map merged");
return;
}
RCLCPP_DEBUG(logger_, "all maps merged, publishing");
// RCLCPP_INFO(logger_, "all maps merged, publishing");
auto now = this->now();
merged_map->info.map_load_time = now;
merged_map->header.stamp = now;
merged_map->header.frame_id = world_frame_;
rcpputils::assert_true(merged_map->info.resolution > 0.f);
merged_map_publisher_->publish(*merged_map);
}
void MapMerge::poseEstimation()
{
RCLCPP_DEBUG(logger_, "Grid pose estimation started.");
RCLCPP_INFO_ONCE(logger_, "Grid pose estimation started.");
std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> grids;
grids.reserve(subscriptions_size_);
{
// We don't lock since because of ROS2 default executor only a callback can run
// boost::shared_lock<boost::shared_mutex> lock(subscriptions_mutex_);
for (auto& subscription : subscriptions_) {
// std::lock_guard<std::mutex> s_lock(subscription.mutex);
grids.push_back(subscription.readonly_map);
}
}
// Print grids size
// RCLCPP_INFO(logger_, "Grids size: %d", grids.size());
std::lock_guard<std::mutex> lock(pipeline_mutex_);
pipeline_.feed(grids.begin(), grids.end());
// TODO allow user to change feature type
bool success = pipeline_.estimateTransforms(combine_grids::FeatureType::AKAZE,
confidence_threshold_);
// bool success = pipeline_.estimateTransforms(combine_grids::FeatureType::SURF,
// confidence_threshold_);
// bool success = pipeline_.estimateTransforms(combine_grids::FeatureType::ORB,
// confidence_threshold_);
if (!success) {
RCLCPP_INFO(logger_, "No grid poses estimated");
}
}
// void MapMerge::fullMapUpdate(const nav_msgs::OccupancyGrid::ConstPtr& msg,
// MapSubscription& subscription)
void MapMerge::fullMapUpdate(const nav_msgs::msg::OccupancyGrid::SharedPtr msg,
MapSubscription& subscription)
{
RCLCPP_DEBUG(logger_, "received full map update");
// RCLCPP_INFO(logger_, "received full map update");
std::lock_guard<std::mutex> lock(subscription.mutex);
if (subscription.readonly_map){
// ros2 header .stamp don't support > operator, we need to create them explicitly
auto t1 = rclcpp::Time(subscription.readonly_map->header.stamp);
auto t2 = rclcpp::Time(msg->header.stamp);
if (t1 > t2) {
// we have been overrunned by faster update. our work was useless.
return;
}
}
subscription.readonly_map = msg;
subscription.writable_map = nullptr;
}
// void MapMerge::partialMapUpdate(
// const map_msgs::OccupancyGridUpdate::ConstPtr& msg,
// MapSubscription& subscription)
void MapMerge::partialMapUpdate(const map_msgs::msg::OccupancyGridUpdate::SharedPtr msg,
MapSubscription& subscription)
{
RCLCPP_DEBUG(logger_, "received partial map update");
if (msg->x < 0 || msg->y < 0) {
RCLCPP_ERROR(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;
nav_msgs::msg::OccupancyGrid::SharedPtr map;
nav_msgs::msg::OccupancyGrid::ConstSharedPtr readonly_map; // local copy
{
// load maps
std::lock_guard<std::mutex> lock(subscription.mutex);
map = subscription.writable_map;
readonly_map = subscription.readonly_map;
}
if (!readonly_map) {
RCLCPP_WARN(logger_, "received partial map update, but don't have any full map to "
"update. skipping.");
return;
}
// // we don't have partial map to take update, we must copy readonly map and
// // update new writable map
if (!map) {
map.reset(new nav_msgs::msg::OccupancyGrid(*readonly_map));
}
size_t grid_xn = map->info.width;
size_t grid_yn = map->info.height;
if (xn > grid_xn || x0 > grid_xn || yn > grid_yn || y0 > grid_yn) {
RCLCPP_WARN(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, grid_xn, grid_yn);
}
// update map with data
size_t i = 0;
for (size_t y = y0; y < yn && y < grid_yn; ++y) {
for (size_t x = x0; x < xn && x < grid_xn; ++x) {
size_t idx = y * grid_xn + x; // index to grid for this specified cell
map->data[idx] = msg->data[i];
++i;
}
}
// update time stamp
map->header.stamp = msg->header.stamp;
{
// store back updated map
std::lock_guard<std::mutex> lock(subscription.mutex);
if (subscription.readonly_map){
// ros2 header .stamp don't support > operator, we need to create them explicitly
auto t1 = rclcpp::Time(subscription.readonly_map->header.stamp);
auto t2 = rclcpp::Time(map->header.stamp);
if (t1 > t2) {
// we have been overrunned by faster update. our work was useless.
return;
}
}
subscription.writable_map = map;
subscription.readonly_map = map;
}
}
std::string MapMerge::robotNameFromTopic(const std::string& topic)
{
return ros1_names::parentNamespace(topic);
}
/* identifies topic via suffix */
bool MapMerge::isRobotMapTopic(const std::string topic, std::string type)
{
/* test whether topic is robot_map_topic_ */
std::string topic_namespace = ros1_names::parentNamespace(topic);
bool is_map_topic = ros1_names::append(topic_namespace, robot_map_topic_) == topic;
// /* test whether topic contains *anywhere* robot namespace */
auto pos = topic.find(robot_namespace_);
bool contains_robot_namespace = pos != std::string::npos;
// /* we support only occupancy grids as maps */
bool is_occupancy_grid = type == "nav_msgs/msg/OccupancyGrid";
// /* we don't want to subcribe on published merged map */
bool is_our_topic = merged_map_publisher_->get_topic_name() == topic;
return is_occupancy_grid && !is_our_topic && contains_robot_namespace &&
is_map_topic;
}
/*
* Get robot's initial position
*/
bool MapMerge::getInitPose(const std::string& name,
geometry_msgs::msg::Transform& pose)
{
std::string merging_namespace = ros1_names::append(name, "map_merge");
double yaw = 0.0;
bool success =
this->get_parameter(ros1_names::append(merging_namespace, "init_pose_x"),
pose.translation.x) &&
this->get_parameter(ros1_names::append(merging_namespace, "init_pose_y"),
pose.translation.y) &&
this->get_parameter(ros1_names::append(merging_namespace, "init_pose_z"),
pose.translation.z) &&
this->get_parameter(ros1_names::append(merging_namespace, "init_pose_yaw"),
yaw);
tf2::Quaternion q;
q.setEuler(0., 0., yaw);
pose.rotation = toMsg(q);
return success;
}
} // namespace map_merge
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
// ROS1 code
// // this package is still in development -- start wil debugging enabled
// if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
// ros::console::levels::Debug)) {
// ros::console::notifyLoggerLevelsChanged();
// }
// ROS2 code
auto node = std::make_shared<map_merge::MapMerge>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}

322
map_merge/test/test_merging_pipeline.cpp Normal file
View File

@ -0,0 +1,322 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2015-2016, Jiri Horner.
* 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 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 <combine_grids/grid_warper.h>
#include <gtest/gtest.h>
#include <ros/console.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <opencv2/core/utility.hpp>
#include "testing_helpers.h"
#define private public
#include <combine_grids/merging_pipeline.h>
const std::array<const char*, 2> hector_maps = {
"map00.pgm",
"map05.pgm",
};
const std::array<const char*, 2> gmapping_maps = {
"2011-08-09-12-22-52.pgm",
"2012-01-28-11-12-01.pgm",
};
constexpr bool verbose_tests = false;
#define EXPECT_VALID_GRID(grid) \
ASSERT_TRUE(static_cast<bool>(grid)); \
EXPECT_TRUE(consistentData(*grid)); \
EXPECT_GT(grid->info.resolution, 0); \
EXPECT_TRUE(isIdentity(grid->info.origin.orientation))
TEST(MergingPipeline, canStich0Grid)
{
std::vector<nav_msgs::OccupancyGridConstPtr> maps;
combine_grids::MergingPipeline merger;
merger.feed(maps.begin(), maps.end());
EXPECT_TRUE(merger.estimateTransforms());
EXPECT_EQ(merger.composeGrids(), nullptr);
EXPECT_EQ(merger.getTransforms().size(), 0);
}
TEST(MergingPipeline, canStich1Grid)
{
auto map = loadMap(hector_maps[1]);
combine_grids::MergingPipeline merger;
merger.feed(&map, &map + 1);
merger.estimateTransforms();
auto merged_grid = merger.composeGrids();
EXPECT_VALID_GRID(merged_grid);
// don't use EXPECT_EQ, since it prints too much info
EXPECT_TRUE(*merged_grid == *map);
// check estimated transforms
auto transforms = merger.getTransforms();
EXPECT_EQ(transforms.size(), 1);
EXPECT_TRUE(isIdentity(transforms[0]));
}
TEST(MergingPipeline, canStich2Grids)
{
auto maps = loadMaps(hector_maps.begin(), hector_maps.end());
combine_grids::MergingPipeline merger;
merger.feed(maps.begin(), maps.end());
merger.estimateTransforms();
auto merged_grid = merger.composeGrids();
EXPECT_VALID_GRID(merged_grid);
// grid size should indicate sucessful merge
EXPECT_NEAR(2091, merged_grid->info.width, 30);
EXPECT_NEAR(2091, merged_grid->info.height, 30);
if (verbose_tests) {
saveMap("test_canStich2Grids.pgm", merged_grid);
}
}
TEST(MergingPipeline, canStichGridsGmapping)
{
auto maps = loadMaps(gmapping_maps.begin(), gmapping_maps.end());
combine_grids::MergingPipeline merger;
merger.feed(maps.begin(), maps.end());
merger.estimateTransforms();
auto merged_grid = merger.composeGrids();
EXPECT_VALID_GRID(merged_grid);
// grid size should indicate sucessful merge
EXPECT_NEAR(5427, merged_grid->info.width, 30);
EXPECT_NEAR(5427, merged_grid->info.height, 30);
if (verbose_tests) {
saveMap("canStichGridsGmapping.pgm", merged_grid);
}
}
TEST(MergingPipeline, estimationAccuracy)
{
// for this test we measure estimation on the same grid artificially
// transformed
double angle = 0.523599 /* 30 deg in rads*/;
// TODO test also translations
double tx = 0;
double ty = 0;
cv::Matx23d transform{std::cos(angle), -std::sin(angle), tx,
std::sin(angle), std::cos(angle), ty};
auto map = loadMap(hector_maps[1]);
combine_grids::MergingPipeline merger;
merger.feed(&map, &map + 1);
// warp the map with Affine Transform
combine_grids::internal::GridWarper warper;
cv::Mat warped;
auto roi = warper.warp(merger.images_[0], cv::Mat(transform), warped);
// add warped map
// this relies on internal implementation of merging pipeline
merger.grids_.emplace_back();
merger.images_.push_back(warped);
merger.estimateTransforms();
auto merged_grid = merger.composeGrids();
EXPECT_VALID_GRID(merged_grid);
// transforms
auto transforms = merger.getTransforms();
EXPECT_EQ(transforms.size(), 2);
EXPECT_TRUE(isIdentity(transforms[0]));
tf2::Transform t;
tf2::fromMsg(transforms[1], t);
EXPECT_NEAR(angle, t.getRotation().getAngle(), 1e-2);
EXPECT_NEAR(tx - roi.tl().x, t.getOrigin().x(), 2);
EXPECT_NEAR(ty - roi.tl().y, t.getOrigin().y(), 2);
}
TEST(MergingPipeline, transformsRoundTrip)
{
auto map = loadMap(hector_maps[0]);
combine_grids::MergingPipeline merger;
merger.feed(&map, &map + 1);
for (size_t i = 0; i < 1000; ++i) {
auto in_transform = randomTransform();
merger.setTransforms(&in_transform, &in_transform + 1);
auto out_transforms = merger.getTransforms();
ASSERT_EQ(out_transforms.size(), 1);
auto out_transform = out_transforms[0];
EXPECT_FLOAT_EQ(in_transform.translation.x, out_transform.translation.x);
EXPECT_FLOAT_EQ(in_transform.translation.y, out_transform.translation.y);
EXPECT_FLOAT_EQ(in_transform.translation.z, out_transform.translation.z);
EXPECT_FLOAT_EQ(in_transform.rotation.x, out_transform.rotation.x);
EXPECT_FLOAT_EQ(in_transform.rotation.y, out_transform.rotation.y);
EXPECT_FLOAT_EQ(in_transform.rotation.z, out_transform.rotation.z);
EXPECT_FLOAT_EQ(in_transform.rotation.w, out_transform.rotation.w);
}
}
TEST(MergingPipeline, setTransformsInternal)
{
auto map = loadMap(hector_maps[0]);
combine_grids::MergingPipeline merger;
merger.feed(&map, &map + 1);
for (size_t i = 0; i < 1000; ++i) {
auto transform = randomTransform();
merger.setTransforms(&transform, &transform + 1);
ASSERT_EQ(merger.transforms_.size(), 1);
auto& transform_internal = merger.transforms_[0];
// verify that transforms are the same in 2D
tf2::Vector3 a[2] = {{1., 0., 1.}, {0., 1., 1.}};
cv::Point3d b[2] = {{1., 0., 1.}, {0., 1., 1.}};
for (auto j : {0, 1}) {
tf2::Transform t;
fromMsg(transform, t);
auto p1 = t * a[j];
cv::Mat p2 = transform_internal * cv::Mat(b[j]);
// some precision is naturally lost during conversion, float precision is
// still good for us
EXPECT_FLOAT_EQ(p1.x(), p2.at<double>(0));
EXPECT_FLOAT_EQ(p1.y(), p2.at<double>(1));
}
}
}
TEST(MergingPipeline, getTransformsInternal)
{
auto map = loadMap(hector_maps[0]);
combine_grids::MergingPipeline merger;
merger.feed(&map, &map + 1);
// set internal transform
merger.transforms_.resize(1);
for (size_t i = 0; i < 1000; ++i) {
cv::Mat transform_internal = randomTransformMatrix();
merger.transforms_[0] = transform_internal;
auto transforms = merger.getTransforms();
ASSERT_EQ(transforms.size(), 1);
// output quaternion should be normalized
auto& q = transforms[0].rotation;
EXPECT_DOUBLE_EQ(1., q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
// verify that transforms are the same in 2D
tf2::Transform transform;
fromMsg(transforms[0], transform);
tf2::Vector3 a[2] = {{1., 0., 1.}, {0., 1., 1.}};
cv::Point3d b[2] = {{1., 0., 1.}, {0., 1., 1.}};
for (auto j : {0, 1}) {
auto p1 = transform * a[j];
cv::Mat p2 = transform_internal * cv::Mat(b[j]);
EXPECT_FLOAT_EQ(p1.x(), p2.at<double>(0));
EXPECT_FLOAT_EQ(p1.y(), p2.at<double>(1));
}
}
}
TEST(MergingPipeline, setEmptyTransforms)
{
constexpr size_t size = 2;
std::vector<nav_msgs::OccupancyGridConstPtr> maps(size);
std::vector<geometry_msgs::Transform> transforms(size);
combine_grids::MergingPipeline merger;
merger.feed(maps.begin(), maps.end());
merger.setTransforms(transforms.begin(), transforms.end());
EXPECT_EQ(merger.composeGrids(), nullptr);
EXPECT_EQ(merger.getTransforms().size(), size);
}
/* empty image may end with identity transform. */
TEST(MergingPipeline, emptyImageWithTransform)
{
constexpr size_t size = 1;
std::vector<nav_msgs::OccupancyGridConstPtr> maps(size);
std::vector<geometry_msgs::Transform> transforms(size);
transforms[0].rotation.z = 1; // set transform to identity
combine_grids::MergingPipeline merger;
merger.feed(maps.begin(), maps.end());
merger.setTransforms(transforms.begin(), transforms.end());
EXPECT_EQ(merger.composeGrids(), nullptr);
EXPECT_EQ(merger.getTransforms().size(), size);
}
/* one image may be empty */
TEST(MergingPipeline, oneEmptyImage)
{
std::vector<nav_msgs::OccupancyGridConstPtr> maps{nullptr,
loadMap(gmapping_maps[0])};
combine_grids::MergingPipeline merger;
merger.feed(maps.begin(), maps.end());
merger.estimateTransforms();
auto merged_grid = merger.composeGrids();
auto transforms = merger.getTransforms();
EXPECT_VALID_GRID(merged_grid);
// don't use EXPECT_EQ, since it prints too much info
EXPECT_TRUE(*merged_grid == *maps[1]);
// transforms
EXPECT_EQ(transforms.size(), 2);
EXPECT_TRUE(isIdentity(transforms[1]));
}
// non-identity known positions etc.
TEST(MergingPipeline, knownInitPositions)
{
auto maps = loadMaps(gmapping_maps.begin(), gmapping_maps.end());
combine_grids::MergingPipeline merger;
merger.feed(maps.begin(), maps.end());
for (size_t i = 0; i < 5; ++i) {
std::vector<geometry_msgs::Transform> transforms{randomTransform(),
randomTransform()};
merger.setTransforms(transforms.begin(), transforms.end());
auto merged_grid = merger.composeGrids();
EXPECT_VALID_GRID(merged_grid);
}
}
int main(int argc, char** argv)
{
ros::Time::init();
if (verbose_tests &&
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
ros::console::levels::Debug)) {
ros::console::notifyLoggerLevelsChanged();
}
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

158
map_merge/test/testing_helpers.h Normal file
View File

@ -0,0 +1,158 @@
#ifndef TESTING_HELPERS_H_
#define TESTING_HELPERS_H_
#include <nav_msgs/OccupancyGrid.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <opencv2/core/utility.hpp>
#include <opencv2/imgcodecs.hpp>
#include <random>
const float resolution = 0.05f;
nav_msgs::OccupancyGridConstPtr loadMap(const std::string& filename);
void saveMap(const std::string& filename,
const nav_msgs::OccupancyGridConstPtr& map);
std::tuple<double, double, double> randomAngleTxTy();
geometry_msgs::Transform randomTransform();
cv::Mat randomTransformMatrix();
/* map_server is really bad. until there is no replacement I will implement it
* by myself */
template <typename InputIt>
std::vector<nav_msgs::OccupancyGridConstPtr> loadMaps(InputIt filenames_begin,
InputIt filenames_end)
{
std::vector<nav_msgs::OccupancyGridConstPtr> result;
for (InputIt it = filenames_begin; it != filenames_end; ++it) {
result.emplace_back(loadMap(*it));
}
return result;
}
nav_msgs::OccupancyGridConstPtr loadMap(const std::string& filename)
{
cv::Mat lookUpTable(1, 256, CV_8S);
signed char* p = lookUpTable.ptr<signed char>();
p[254] = 0;
p[205] = -1;
p[0] = 100;
cv::Mat img = cv::imread(filename, cv::IMREAD_GRAYSCALE);
if (img.empty()) {
throw std::runtime_error("could not load map");
}
nav_msgs::OccupancyGridPtr grid{new nav_msgs::OccupancyGrid()};
grid->info.width = static_cast<uint>(img.size().width);
grid->info.height = static_cast<uint>(img.size().height);
grid->info.resolution = resolution;
grid->data.resize(static_cast<size_t>(img.size().area()));
cv::Mat grid_view(img.size(), CV_8S,
const_cast<signed char*>(grid->data.data()));
cv::LUT(img, lookUpTable, grid_view);
return grid;
}
void saveMap(const std::string& filename,
const nav_msgs::OccupancyGridConstPtr& map)
{
cv::Mat lookUpTable(1, 256, CV_8U);
uchar* p = lookUpTable.ptr();
for (int i = 0; i < 255; ++i) {
if (i >= 0 && i < 10)
p[i] = 254;
else
p[i] = 0;
}
p[255] = 205;
cv::Mat img(map->info.height, map->info.width, CV_8S,
const_cast<signed char*>(map->data.data()));
cv::Mat out_img;
cv::LUT(img, lookUpTable, out_img);
cv::imwrite(filename, out_img);
}
std::tuple<double, double, double> randomAngleTxTy()
{
static std::mt19937_64 g(156468754 /*magic*/);
std::uniform_real_distribution<double> rotation_dis(0., 2 * std::acos(-1));
std::uniform_real_distribution<double> translation_dis(-1000, 1000);
return std::tuple<double, double, double>(rotation_dis(g), translation_dis(g),
translation_dis(g));
}
geometry_msgs::Transform randomTransform()
{
double angle, tx, ty;
std::tie(angle, tx, ty) = randomAngleTxTy();
tf2::Transform transform;
tf2::Quaternion rotation;
rotation.setEuler(0., 0., angle);
rotation.normalize();
transform.setRotation(rotation);
transform.setOrigin(tf2::Vector3(tx, ty, 0.));
auto msg = toMsg(transform);
// normalize quaternion such that w > 0 (q and -q represents the same
// transformation)
if (msg.rotation.w < 0.) {
msg.rotation.x *= -1.;
msg.rotation.y *= -1.;
msg.rotation.z *= -1.;
msg.rotation.w *= -1.;
}
return msg;
}
cv::Mat randomTransformMatrix()
{
double angle, tx, ty;
std::tie(angle, tx, ty) = randomAngleTxTy();
cv::Mat transform =
(cv::Mat_<double>(3, 3) << std::cos(angle), -std::sin(angle), tx,
std::sin(angle), std::cos(angle), ty, 0., 0., 1.);
return transform;
}
static inline bool isIdentity(const geometry_msgs::Transform& transform)
{
tf2::Transform t;
tf2::fromMsg(transform, t);
return tf2::Transform::getIdentity() == t;
}
static inline bool isIdentity(const geometry_msgs::Quaternion& rotation)
{
tf2::Quaternion q;
tf2::fromMsg(rotation, q);
return tf2::Quaternion::getIdentity() == q;
}
// data size is consistent with height and width
static inline bool consistentData(const nav_msgs::OccupancyGrid& grid)
{
return grid.info.width * grid.info.height == grid.data.size();
}
// ignores header, map_load_time and origin
static inline bool operator==(const nav_msgs::OccupancyGrid& grid1,
const nav_msgs::OccupancyGrid& grid2)
{
bool equal = true;
equal &= grid1.info.width == grid2.info.width;
equal &= grid1.info.height == grid2.info.height;
equal &= std::abs(grid1.info.resolution - grid2.info.resolution) <
std::numeric_limits<float>::epsilon();
equal &= grid1.data.size() == grid2.data.size();
for (size_t i = 0; i < grid1.data.size(); ++i) {
equal &= grid1.data[i] == grid2.data[i];
}
return equal;
}
#endif // TESTING_HELPERS_H_