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

Compare commits

base: zcy:9c121aebe52d28f3846a1cbb6ebb017cc85f9e53
Branches Tags
zcy:main
..
compare: zcy:b9a5a5ecaa45fae8d2c0cd2dee0dca27e4ecdd67
Branches Tags
zcy:main

No commits in common. "9c121aebe52d28f3846a1cbb6ebb017cc85f9e53" and "b9a5a5ecaa45fae8d2c0cd2dee0dca27e4ecdd67" have entirely different histories.
9c121aebe5 ... b9a5a5ecaa

22 changed files with 144 additions and 737 deletions
Show Stats Expand all files Collapse all files

129
.devcontainer/Dockerfile
View File

@ -1,129 +0,0 @@
# FROM nvidia/cuda:11.7.1-devel-ubuntu22.04
FROM ubuntu:jammy
# FROM nvidia/cuda:11.1.1-cudnn8-devel-ubuntu20.04
# FROM ubuntu:focal
ENV DEBIAN_FRONTEND noninteractive
RUN apt-get update && apt-get install --no-install-recommends -y \
apt-utils \
bzip2 \
lbzip2 \
tar \
wget \
libzbar0 \
unzip \
build-essential \
zlib1g-dev \
libcurl4-gnutls-dev \
locales \
curl \
gnupg2 \
lsb-release \
ca-certificates \
&& apt autoremove -y && apt clean -y \
&& rm -rf /var/lib/apt/lists/*
# https://index.ros.org/doc/ros2/Installation/Crystal/Linux-Install-Debians/
ENV ROS_DISTRO=humble
ENV LANG=C.UTF-8
ENV LC_ALL=C.UTF-8
RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg \
&& echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null
RUN sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' \
&& wget https://packages.osrfoundation.org/gazebo.key -O - | apt-key add - \
&& apt update && apt install --no-install-recommends -y \
ros-${ROS_DISTRO}-ros-base \
ros-${ROS_DISTRO}-rclcpp-cascade-lifecycle \
ros-${ROS_DISTRO}-geographic-msgs \
ros-${ROS_DISTRO}-camera-info-manager \
ros-${ROS_DISTRO}-launch-testing-ament-cmake \
ros-${ROS_DISTRO}-diagnostic-updater \
ros-${ROS_DISTRO}-rviz2 \
ros-${ROS_DISTRO}-gazebo-ros \
ros-${ROS_DISTRO}-gazebo-ros-pkgs \
ros-${ROS_DISTRO}-gazebo-msgs \
ros-${ROS_DISTRO}-gazebo-plugins \
ros-${ROS_DISTRO}-robot-state-publisher \
ros-${ROS_DISTRO}-cv-bridge \
ros-${ROS_DISTRO}-message-filters \
ros-${ROS_DISTRO}-image-transport \
ros-${ROS_DISTRO}-rqt* \
ros-${ROS_DISTRO}-slam-toolbox \
ros-${ROS_DISTRO}-navigation2 \
ros-${ROS_DISTRO}-nav2-bringup \
ros-${ROS_DISTRO}-behaviortree-cpp-v3 \
ros-${ROS_DISTRO}-angles \
ros-${ROS_DISTRO}-ompl \
ros-${ROS_DISTRO}-turtlebot3* \
ros-${ROS_DISTRO}-image-geometry \
&& apt autoremove && apt clean -y \
&& rm -rf /var/lib/apt/lists/*
# Install ROS2 gazebo dependencies
RUN apt update && apt-get install --no-install-recommends -y \
libglvnd0 \
libglx0 \
libegl1 \
libxext6 \
libx11-6 \
libblkid-dev \
e2fslibs-dev \
libboost-all-dev \
libaudit-dev \
git \
nano \
# ------------------------------
&& apt autoremove && apt clean -y \
&& rm -rf /var/lib/apt/lists/*
RUN apt update && apt install --no-install-recommends -y \
python3-dev \
python3-pip \
python3-colcon-common-extensions \
&& pip3 install rosdep \
&& rosdep init \
&& rosdep update \
&& apt autoremove && apt clean -y \
&& rm -rf /var/lib/apt/lists/*
# Or your actual UID, GID on Linux if not the default 1000
ARG USERNAME=dev
ARG USER_UID=1000
ARG USER_GID=$USER_UID
# Create a non-root user to use if preferred - see https://aka.ms/vscode-remote/containers/non-root-user.
RUN groupadd --gid $USER_GID $USERNAME \
&& useradd -s /bin/bash --uid $USER_UID --gid $USER_GID -m $USERNAME \
# Add sudo support for non-root user
&& apt-get update && apt-get install -y sudo \
&& echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \
&& chmod 0440 /etc/sudoers.d/$USERNAME \
&& apt-get autoremove && apt-get clean -y \
&& rm -rf /var/lib/apt/lists/*
RUN \
mkdir -p /home/${USERNAME}/.ignition/fuel/ \
&& echo "servers:\n -\n name: osrf\n url: https://api.ignitionrobotics.org" >> /home/${USERNAME}/.ignition/fuel/config.yaml \
&& chown ${USERNAME} /home/${USERNAME}/.ignition \
&& GAZEBO_SOURCE="source /usr/share/gazebo/setup.sh" \
&& echo $GAZEBO_SOURCE >> "/home/${USERNAME}/.bashrc" \
&& chown ${USERNAME} /home/${USERNAME}/.ignition
# ROS2 source setup
RUN ROS_SOURCE="source /opt/ros/${ROS_DISTRO}/setup.sh" \
&& echo $ROS_SOURCE >> "/home/${USERNAME}/.bashrc"
WORKDIR /workspace/ros_ws/src
# Give permission to non-root user to access the workspace
RUN chown -R ${USERNAME} /workspace/ros_ws
RUN git clone https://github.com/charlielito/slam_gmapping.git --branch feature/namespace_launch
ENV TURTLEBOT3_MODEL=waffle
ENV GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/${ROS_DISTRO}/share/turtlebot3_gazebo/models
# Switch back to dialog for any ad-hoc use of apt-get
ENV DEBIAN_FRONTEND=
CMD ["/bin/bash"]

24
.devcontainer/devcontainer.json
View File

@ -1,24 +0,0 @@
{
"name": "Dev container",
"dockerComposeFile": "docker-compose.yml",
"service": "m-explore-ros2-humble",
"workspaceFolder": "/workspace/ros_ws",
"runArgs": [
"--runtime=nvidia"
],
"settings": {
"terminal.integrated.automationShell.linux": "/bin/bash",
},
"extensions": [
"ms-azuretools.vscode-docker",
"eamodio.gitlens",
"streetsidesoftware.code-spell-checker",
"oderwat.indent-rainbow",
"ms-vsliveshare.vsliveshare",
"pkief.material-icon-theme",
"ms-python.python",
"ms-vscode.cpptools",
"xaver.clang-format",
"ms-python.vscode-pylance",
],
}

53
.devcontainer/docker-compose.yml
View File

@ -1,53 +0,0 @@
# version: "3.7"
version: "2.3"
services:
m-explore-ros2-humble:
runtime: nvidia
build:
context: ../
dockerfile: .devcontainer/Dockerfile
working_dir: /workspace/ros_ws
user: dev
network_mode: host
ports:
- "80:80"
expose:
- 80
init: true
privileged: true
environment:
- DISPLAY=$DISPLAY
- QT_X11_NO_MITSHM=1
- UDEV=1
- NVIDIA_DRIVER_CAPABILITIES=compute,utility,display
volumes:
# Update this to wherever you want VS Code to mount the folder of your project
- ..:/workspace/ros_ws/src/m-explore-ros2
# Forwards the local Docker socket to the container.
- /var/run/docker.sock:/var/run/docker.sock
# Enable GUI environments
- /tmp/.X11-unix:/tmp/.X11-unix:rw
devices:
- /dev/bus/usb:/dev/bus/usb
# NVIDIA drivers to use OpenGL, etc...
- /dev/nvidia0:/dev/nvidia0
- /dev/nvidiactl:/dev/nvidiactl
- /dev/nvidia-uvm:/dev/nvidia-uvm
- /dev/input:/dev/input
- /dev/snd:/dev/snd
# Uncomment the next four lines if you will use a ptrace-based debuggers like C++, Go, and Rust.
cap_add:
- SYS_PTRACE
security_opt:
- seccomp:unconfined
# Overrides default command so things don't shut down after the process ends.
stdin_open: true
tty: true
command: "/bin/bash"

47
.github/workflows/build_test.yaml vendored
View File

@ -1,47 +0,0 @@
name: ROS2 build/tests
on:
pull_request:
jobs:
build_and_tests:
runs-on: ubuntu-20.04
strategy:
matrix:
ros_distribution:
- foxy
- galactic
- humble
include:
# Foxy Fitzroy (June 2020 - May 2023)
- docker_image: ubuntu:focal
ros_distribution: foxy
# Galactic Geochelone (May 2021 - November 2022)
- docker_image: ubuntu:focal
ros_distribution: galactic
# Humble Hawksbill (May 2022 - May 2027)
- docker_image: ubuntu:jammy
ros_distribution: humble
container:
image: ${{ matrix.docker_image }}
steps:
- name: setup ROS environment
uses: ros-tooling/setup-ros@v0.4
with:
required-ros-distributions: ${{ matrix.ros_distribution }}
- name: Checkout repository
uses: actions/checkout@v3
- name: build multirobot_map_merge and explore_lite
uses: ros-tooling/action-ros-ci@v0.2
with:
package-name: multirobot_map_merge explore_lite
target-ros2-distro: ${{ matrix.ros_distribution }}
skip-tests: true
- name: Run gtests manually multirobot_map_merge
run: |
. /opt/ros/${{ matrix.ros_distribution }}/setup.sh && . ros_ws/install/setup.sh
cd ros_ws/build/multirobot_map_merge
./test_merging_pipeline
- name: Run gtests manually explore_lite
run: |
. /opt/ros/${{ matrix.ros_distribution }}/setup.sh && . ros_ws/install/setup.sh
cd ros_ws/build/explore_lite
./test_explore

19
README.md
View File

@ -38,7 +38,7 @@ RUNNING
------- -------
To run with a params file just run it with To run with a params file just run it with
``` ```
ros2 run explore_lite explore --ros-args --params-file <path_to_ros_ws>/m-explore-ros2/explore/config/params.yaml ros2 run explore_lite explore --ros-args --params-file <path_to_ros_ws>/m-explore/explore/config/params.yaml
``` ```
### Running the explore demo with TB3 ### Running the explore demo with TB3
@ -49,7 +49,7 @@ Then just run the nav2 stack with slam:
``` ```
export TURTLEBOT3_MODEL=waffle export TURTLEBOT3_MODEL=waffle
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/${ROS_DISTRO}/share/turtlebot3_gazebo/models 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
``` ```
And run this package with And run this package with
@ -57,14 +57,7 @@ And run this package with
ros2 launch explore_lite explore.launch.py ros2 launch explore_lite explore.launch.py
``` ```
You can open rviz2 and add the exploration frontiers marker (topic is `explore/frontiers`) to see the algorithm working and the frontier chosen to explore. You can open a rviz2 and add the exploration frontiers marker to see the algorithm working and choose a frontier to explore.
### Additional features
#### Stop/Resume exploration
By default the exploration node will start right away the frontier-based exploration algorithm. Alternatively, you can stop the exploration by publishing to a `False` to `explore/resume` topic. This will stop the exploration and the robot will stop moving. You can resume the exploration by publishing to `True` to `explore/resume`.
#### Returning to initial pose
The robot will return to its initial pose after exploration if you want by defining the parameter `return_to_init` to `True` when launching the node.
#### TB3 troubleshooting (with foxy) #### TB3 troubleshooting (with foxy)
If you have trouble with TB3 in simulation, as we did, add these extra steps for configuring it. If you have trouble with TB3 in simulation, as we did, add these extra steps for configuring it.
@ -115,12 +108,10 @@ colcon build --symlink-install --packages-up-to slam_gmapping
**Note**: You could use [slam_toolbox](https://github.com/SteveMacenski/slam_toolbox) instead but you need to use this [experimental branch](https://github.com/robo-friends/m-explore-ros2/tree/feature/slam_toolbox_compat) which is still under development. **Note**: You could use [slam_toolbox](https://github.com/SteveMacenski/slam_toolbox) instead but you need to use this [experimental branch](https://github.com/robo-friends/m-explore-ros2/tree/feature/slam_toolbox_compat) which is still under development.
#### Nav2 gazebo spawner (deprecated in humble) #### Nav2 gazebo spawner
To spawn multiple robots, you need the `nav2_gazebo_spawner` which does not come up with the `nav2-bringup` installation. For that, install it with `sudo apt install ros-${ROS_DISTRO}-nav2-gazebo-spawner`. To spawn multiple robots, you need the `nav2_gazebo_spawner` which does not come up with the `nav2-bringup` installation. For that, install it with `sudo apt install ros-${ROS_DISTRO}-nav2-gazebo-spawner`.
Note that was the case for release previous to `humble` but since `humble` release, this package is deprecated and a gazebo node is used for this. So, if you are using `humble` or newer, you don't need to install it.
#### Nav2 config files #### 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/humble distros and since some breaking changes were introduced in this distro, 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). 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 ### 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:: First, you'll need to launch the whole simulation stack, nav2 stacks and slam stacks per robot. For that just launch::

21
explore/CMakeLists.txt
View File

@ -88,7 +88,7 @@ target_include_directories(explore PUBLIC
$<INSTALL_INTERFACE:include>) $<INSTALL_INTERFACE:include>)
target_link_libraries(explore ${rclcpp_LIBRARIES}) target_link_libraries(explore)
ament_target_dependencies(explore ${DEPENDENCIES}) ament_target_dependencies(explore ${DEPENDENCIES})
@ -97,25 +97,6 @@ install(TARGETS explore
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
#############
## Testing ##
#############
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
set(ament_cmake_copyright_FOUND TRUE)
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
find_package(ament_cmake_gtest REQUIRED)
ament_add_gtest(test_explore test/test_explore.cpp)
target_link_libraries(test_explore ${catkin_LIBRARIES})
ament_target_dependencies(test_explore ${DEPENDENCIES})
endif()
ament_export_include_directories(include) ament_export_include_directories(include)
ament_package() ament_package()

3
explore/config/params.yaml
View File

@ -1,11 +1,10 @@
/**: /**:
ros__parameters: ros__parameters:
robot_base_frame: base_link robot_base_frame: base_link
return_to_init: true
costmap_topic: map costmap_topic: map
costmap_updates_topic: map_updates costmap_updates_topic: map_updates
visualize: true visualize: true
planner_frequency: 0.15 planner_frequency: 0.25
progress_timeout: 30.0 progress_timeout: 30.0
potential_scale: 3.0 potential_scale: 3.0
orientation_scale: 0.0 orientation_scale: 0.0

2
explore/config/params_costmap.yaml
View File

@ -4,7 +4,7 @@ explore_node:
costmap_topic: /global_costmap/costmap costmap_topic: /global_costmap/costmap
costmap_updates_topic: /global_costmap/costmap_updates costmap_updates_topic: /global_costmap/costmap_updates
visualize: true visualize: true
planner_frequency: 0.15 planner_frequency: 0.25
progress_timeout: 30.0 progress_timeout: 30.0
potential_scale: 3.0 potential_scale: 3.0
orientation_scale: 0.0 orientation_scale: 0.0

13
explore/include/explore/explore.h
View File

@ -49,7 +49,6 @@
#include <memory> #include <memory>
#include <mutex> #include <mutex>
#include <rclcpp/rclcpp.hpp> #include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/bool.hpp>
#include <std_msgs/msg/color_rgba.hpp> #include <std_msgs/msg/color_rgba.hpp>
#include <string> #include <string>
#include <vector> #include <vector>
@ -80,8 +79,7 @@ public:
~Explore(); ~Explore();
void start(); void start();
void stop(bool finished_exploring = false); void stop();
void resume();
using NavigationGoalHandle = using NavigationGoalHandle =
rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateToPose>; rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateToPose>;
@ -120,26 +118,17 @@ private:
rclcpp::TimerBase::SharedPtr exploring_timer_; rclcpp::TimerBase::SharedPtr exploring_timer_;
// rclcpp::TimerBase::SharedPtr oneshot_; // rclcpp::TimerBase::SharedPtr oneshot_;
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr resume_subscription_;
void resumeCallback(const std_msgs::msg::Bool::SharedPtr msg);
std::vector<geometry_msgs::msg::Point> frontier_blacklist_; std::vector<geometry_msgs::msg::Point> frontier_blacklist_;
geometry_msgs::msg::Point prev_goal_; geometry_msgs::msg::Point prev_goal_;
double prev_distance_; double prev_distance_;
rclcpp::Time last_progress_; rclcpp::Time last_progress_;
size_t last_markers_count_; size_t last_markers_count_;
geometry_msgs::msg::Pose initial_pose_;
void returnToInitialPose(void);
// parameters // parameters
double planner_frequency_; double planner_frequency_;
double potential_scale_, orientation_scale_, gain_scale_; double potential_scale_, orientation_scale_, gain_scale_;
double progress_timeout_; double progress_timeout_;
bool visualize_; bool visualize_;
bool return_to_init_;
std::string robot_base_frame_;
bool resuming_ = false;
}; };
} // namespace explore } // namespace explore

2
explore/src/costmap_client.cpp
View File

@ -36,11 +36,11 @@
*********************************************************************/ *********************************************************************/
#include <explore/costmap_client.h> #include <explore/costmap_client.h>
#include <unistd.h>
#include <functional> #include <functional>
#include <mutex> #include <mutex>
#include <string> #include <string>
#include <unistd.h>
namespace explore namespace explore
{ {

118
explore/src/explore.cpp
View File

@ -40,15 +40,6 @@
#include <thread> #include <thread>
inline static bool same_point(const geometry_msgs::msg::Point& one,
const geometry_msgs::msg::Point& two)
{
double dx = one.x - two.x;
double dy = one.y - two.y;
double dist = sqrt(dx * dx + dy * dy);
return dist < 0.01;
}
namespace explore namespace explore
{ {
Explore::Explore() Explore::Explore()
@ -68,7 +59,6 @@ Explore::Explore()
this->declare_parameter<float>("orientation_scale", 0.0); this->declare_parameter<float>("orientation_scale", 0.0);
this->declare_parameter<float>("gain_scale", 1.0); this->declare_parameter<float>("gain_scale", 1.0);
this->declare_parameter<float>("min_frontier_size", 0.5); this->declare_parameter<float>("min_frontier_size", 0.5);
this->declare_parameter<bool>("return_to_init", false);
this->get_parameter("planner_frequency", planner_frequency_); this->get_parameter("planner_frequency", planner_frequency_);
this->get_parameter("progress_timeout", timeout); this->get_parameter("progress_timeout", timeout);
@ -77,13 +67,9 @@ Explore::Explore()
this->get_parameter("orientation_scale", orientation_scale_); this->get_parameter("orientation_scale", orientation_scale_);
this->get_parameter("gain_scale", gain_scale_); this->get_parameter("gain_scale", gain_scale_);
this->get_parameter("min_frontier_size", min_frontier_size); this->get_parameter("min_frontier_size", min_frontier_size);
this->get_parameter("return_to_init", return_to_init_);
this->get_parameter("robot_base_frame", robot_base_frame_);
progress_timeout_ = timeout; progress_timeout_ = timeout;
move_base_client_ = move_base_client_ =
rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>( rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(this,ACTION_NAME);
this, ACTION_NAME);
search_ = frontier_exploration::FrontierSearch(costmap_client_.getCostmap(), search_ = frontier_exploration::FrontierSearch(costmap_client_.getCostmap(),
potential_scale_, gain_scale_, potential_scale_, gain_scale_,
@ -91,43 +77,18 @@ Explore::Explore()
if (visualize_) { if (visualize_) {
marker_array_publisher_ = marker_array_publisher_ =
this->create_publisher<visualization_msgs::msg::MarkerArray>("explore/" this->create_publisher<visualization_msgs::msg::MarkerArray>("frontier"
"frontier"
"s", "s",
10); 10);
} }
// Subscription to resume or stop exploration
resume_subscription_ = this->create_subscription<std_msgs::msg::Bool>(
"explore/resume", 10,
std::bind(&Explore::resumeCallback, this, std::placeholders::_1));
RCLCPP_INFO(logger_, "Waiting to connect to move_base nav2 server"); RCLCPP_INFO(logger_, "Waiting to connect to move_base nav2 server");
move_base_client_->wait_for_action_server(); move_base_client_->wait_for_action_server();
RCLCPP_INFO(logger_, "Connected to move_base nav2 server"); RCLCPP_INFO(logger_, "Connected to move_base nav2 server");
if (return_to_init_) {
RCLCPP_INFO(logger_, "Getting initial pose of the robot");
geometry_msgs::msg::TransformStamped transformStamped;
std::string map_frame = costmap_client_.getGlobalFrameID();
try {
transformStamped = tf_buffer_.lookupTransform(
map_frame, robot_base_frame_, tf2::TimePointZero);
initial_pose_.position.x = transformStamped.transform.translation.x;
initial_pose_.position.y = transformStamped.transform.translation.y;
initial_pose_.orientation = transformStamped.transform.rotation;
} catch (tf2::TransformException& ex) {
RCLCPP_ERROR(logger_, "Couldn't find transform from %s to %s: %s",
map_frame.c_str(), robot_base_frame_.c_str(), ex.what());
return_to_init_ = false;
}
}
exploring_timer_ = this->create_wall_timer( exploring_timer_ = this->create_wall_timer(
std::chrono::milliseconds((uint16_t)(1000.0 / planner_frequency_)), std::chrono::milliseconds((uint16_t)(1000.0 / planner_frequency_)),
[this]() { makePlan(); }); [this]() { makePlan(); });
// Start exploration right away
makePlan();
} }
Explore::~Explore() Explore::~Explore()
@ -135,15 +96,6 @@ Explore::~Explore()
stop(); stop();
} }
void Explore::resumeCallback(const std_msgs::msg::Bool::SharedPtr msg)
{
if (msg->data) {
resume();
} else {
stop();
}
}
void Explore::visualizeFrontiers( void Explore::visualizeFrontiers(
const std::vector<frontier_exploration::Frontier>& frontiers) const std::vector<frontier_exploration::Frontier>& frontiers)
{ {
@ -186,8 +138,7 @@ void Explore::visualizeFrontiers(
#else #else
m.lifetime = rclcpp::Duration::from_seconds(0); // foxy onwards m.lifetime = rclcpp::Duration::from_seconds(0); // foxy onwards
#endif #endif
// m.lifetime = rclcpp::Duration::from_nanoseconds(0); // suggested in // m.lifetime = rclcpp::Duration::from_nanoseconds(0); // suggested in galactic
// galactic
m.frame_locked = true; m.frame_locked = true;
// weighted frontiers are always sorted // weighted frontiers are always sorted
@ -198,7 +149,7 @@ void Explore::visualizeFrontiers(
for (auto& frontier : frontiers) { for (auto& frontier : frontiers) {
m.type = visualization_msgs::msg::Marker::POINTS; m.type = visualization_msgs::msg::Marker::POINTS;
m.id = int(id); m.id = int(id);
// m.pose.position = {}; // compile warning m.pose.position = {};
m.scale.x = 0.1; m.scale.x = 0.1;
m.scale.y = 0.1; m.scale.y = 0.1;
m.scale.z = 0.1; m.scale.z = 0.1;
@ -248,8 +199,7 @@ void Explore::makePlan()
} }
if (frontiers.empty()) { if (frontiers.empty()) {
RCLCPP_WARN(logger_, "No frontiers found, stopping."); stop();
stop(true);
return; return;
} }
@ -265,14 +215,13 @@ void Explore::makePlan()
return goalOnBlacklist(f.centroid); return goalOnBlacklist(f.centroid);
}); });
if (frontier == frontiers.end()) { if (frontier == frontiers.end()) {
RCLCPP_WARN(logger_, "All frontiers traversed/tried out, stopping."); stop();
stop(true);
return; return;
} }
geometry_msgs::msg::Point target_position = frontier->centroid; geometry_msgs::msg::Point target_position = frontier->centroid;
// time out if we are not making any progress // time out if we are not making any progress
bool same_goal = same_point(prev_goal_, target_position); bool same_goal = prev_goal_ == target_position;
prev_goal_ = target_position; prev_goal_ = target_position;
if (!same_goal || prev_distance_ > frontier->min_distance) { if (!same_goal || prev_distance_ > frontier->min_distance) {
@ -281,19 +230,15 @@ void Explore::makePlan()
prev_distance_ = frontier->min_distance; prev_distance_ = frontier->min_distance;
} }
// black list if we've made no progress for a long time // black list if we've made no progress for a long time
if ((this->now() - last_progress_ > if (this->now() - last_progress_ >
tf2::durationFromSec(progress_timeout_)) && !resuming_) { tf2::durationFromSec(progress_timeout_)) { // TODO: is progress_timeout_
// in seconds?
frontier_blacklist_.push_back(target_position); frontier_blacklist_.push_back(target_position);
RCLCPP_DEBUG(logger_, "Adding current goal to black list"); RCLCPP_DEBUG(logger_, "Adding current goal to black list");
makePlan(); makePlan();
return; return;
} }
// ensure only first call of makePlan was set resuming to true
if (resuming_) {
resuming_ = false;
}
// we don't need to do anything if we still pursuing the same goal // we don't need to do anything if we still pursuing the same goal
if (same_goal) { if (same_goal) {
return; return;
@ -322,20 +267,6 @@ void Explore::makePlan()
move_base_client_->async_send_goal(goal, send_goal_options); move_base_client_->async_send_goal(goal, send_goal_options);
} }
void Explore::returnToInitialPose()
{
RCLCPP_INFO(logger_, "Returning to initial pose.");
auto goal = nav2_msgs::action::NavigateToPose::Goal();
goal.pose.pose.position = initial_pose_.position;
goal.pose.pose.orientation = initial_pose_.orientation;
goal.pose.header.frame_id = costmap_client_.getGlobalFrameID();
goal.pose.header.stamp = this->now();
auto send_goal_options =
rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SendGoalOptions();
move_base_client_->async_send_goal(goal, send_goal_options);
}
bool Explore::goalOnBlacklist(const geometry_msgs::msg::Point& goal) bool Explore::goalOnBlacklist(const geometry_msgs::msg::Point& goal)
{ {
constexpr static size_t tolerace = 5; constexpr static size_t tolerace = 5;
@ -364,16 +295,13 @@ void Explore::reachedGoal(const NavigationGoalHandle::WrappedResult& result,
RCLCPP_DEBUG(logger_, "Goal was aborted"); RCLCPP_DEBUG(logger_, "Goal was aborted");
frontier_blacklist_.push_back(frontier_goal); frontier_blacklist_.push_back(frontier_goal);
RCLCPP_DEBUG(logger_, "Adding current goal to black list"); RCLCPP_DEBUG(logger_, "Adding current goal to black list");
// If it was aborted probably because we've found another frontier goal,
// so just return and don't make plan again
return; return;
case rclcpp_action::ResultCode::CANCELED: case rclcpp_action::ResultCode::CANCELED:
RCLCPP_DEBUG(logger_, "Goal was canceled"); RCLCPP_DEBUG(logger_, "Goal was canceled");
// If goal canceled might be because exploration stopped from topic. Don't make new plan.
return; return;
default: default:
RCLCPP_WARN(logger_, "Unknown result code from move base nav2"); RCLCPP_WARN(logger_, "Unknown result code from move base nav2");
break; return;
} }
// find new goal immediately regardless of planning frequency. // find new goal immediately regardless of planning frequency.
// execute via timer to prevent dead lock in move_base_client (this is // execute via timer to prevent dead lock in move_base_client (this is
@ -383,9 +311,9 @@ void Explore::reachedGoal(const NavigationGoalHandle::WrappedResult& result,
// ros::Duration(0, 0), [this](const ros::TimerEvent&) { makePlan(); }, // ros::Duration(0, 0), [this](const ros::TimerEvent&) { makePlan(); },
// true); // true);
// Because of the 1-thread-executor nature of ros2 I think timer is not // TODO: Implement this with ros2 timers?
// needed. // Because of the async nature of ros2 calls I think this is not needed.
makePlan(); // makePlan();
} }
void Explore::start() void Explore::start()
@ -393,25 +321,11 @@ void Explore::start()
RCLCPP_INFO(logger_, "Exploration started."); RCLCPP_INFO(logger_, "Exploration started.");
} }
void Explore::stop(bool finished_exploring) void Explore::stop()
{ {
RCLCPP_INFO(logger_, "Exploration stopped.");
move_base_client_->async_cancel_all_goals(); move_base_client_->async_cancel_all_goals();
exploring_timer_->cancel(); exploring_timer_->cancel();
RCLCPP_INFO(logger_, "Exploration stopped.");
if (return_to_init_ && finished_exploring) {
returnToInitialPose();
}
}
void Explore::resume()
{
resuming_ = true;
RCLCPP_INFO(logger_, "Exploration resuming.");
// Reactivate the timer
exploring_timer_->reset();
// Resume immediately
makePlan();
} }
} // namespace explore } // namespace explore

77
explore/test/test_explore.cpp
View File

@ -1,77 +0,0 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2022, Carlos Alvarez.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Carlos Alvarez nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*********************************************************************/
#include <gtest/gtest.h>
#include <geometry_msgs/msg/point.hpp>
#include <cmath>
#define private public
inline static bool same_point(const geometry_msgs::msg::Point& one,
const geometry_msgs::msg::Point& two)
{
double dx = one.x - two.x;
double dy = one.y - two.y;
double dist = sqrt(dx * dx + dy * dy);
return dist < 0.01;
}
TEST(Explore, testSameGoal)
{
geometry_msgs::msg::Point goal1;
geometry_msgs::msg::Point goal2;
// Populate the goal with known values
goal1.x = 1.0;
goal1.y = 2.0;
goal1.z = 3.0;
goal2.x = 0.0;
goal2.y = 0.0;
goal2.z = 0.0;
auto same_goal = same_point(goal1, goal2);
EXPECT_FALSE(same_goal);
goal2.x = goal1.x;
goal2.y = goal1.y;
goal2.z = goal1.z;
same_goal = same_point(goal1, goal2);
EXPECT_TRUE(same_goal);
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

66
map_merge/CMakeLists.txt
View File

@ -23,8 +23,6 @@ find_package(image_geometry REQUIRED)
find_package(map_msgs REQUIRED) find_package(map_msgs REQUIRED)
find_package(nav_msgs REQUIRED) find_package(nav_msgs REQUIRED)
find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_geometry_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(Boost REQUIRED COMPONENTS thread) find_package(Boost REQUIRED COMPONENTS thread)
@ -45,8 +43,6 @@ set(DEPENDENCIES
map_msgs map_msgs
nav_msgs nav_msgs
tf2_geometry_msgs tf2_geometry_msgs
tf2
tf2_ros
OpenCV OpenCV
) )
@ -99,7 +95,6 @@ target_link_libraries(map_merge combine_grids)
# target_link_libraries(map_merge) # target_link_libraries(map_merge)
ament_target_dependencies(map_merge ${DEPENDENCIES}) ament_target_dependencies(map_merge ${DEPENDENCIES})
ament_target_dependencies(combine_grids ${DEPENDENCIES})
install( install(
TARGETS combine_grids TARGETS combine_grids
@ -115,50 +110,35 @@ install(TARGETS map_merge
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
ament_export_include_directories(include)
ament_export_libraries(combine_grids)
ament_package()
############# #############
## Testing ## ## Testing ##
############# #############
if(BUILD_TESTING) # if(CATKIN_ENABLE_TESTING)
find_package(ament_lint_auto REQUIRED) # find_package(roslaunch REQUIRED)
# the following line skips the linter which checks for copyrights
set(ament_cmake_copyright_FOUND TRUE)
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
find_package(ament_cmake_gtest 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)
# download test data: TODO: ROS2 alternative? For now you'll need to download them manually # catkin_add_gtest(test_merging_pipeline test/test_merging_pipeline.cpp)
set(base_url https://raw.githubusercontent.com/hrnr/m-explore-extra/master/map_merge) # # ensure that test data are downloaded before we run tests
# ament_download(${base_url}/hector_maps/map00.pgm # 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)
# MD5 915609a85793ec1375f310d44f2daf87 # target_link_libraries(test_merging_pipeline combine_grids ${catkin_LIBRARIES})
# FILENAME ${PROJECT_NAME}_map00.pgm
# ) # # test all launch files
execute_process( # # do not test from_map_server.launch as we don't want to add dependency on map_server and this
COMMAND bash download.sh ${CMAKE_BINARY_DIR}/map00.pgm ${base_url}/hector_maps/map00.pgm 915609a85793ec1375f310d44f2daf87 # # launchfile is not critical
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/test # roslaunch_add_file_check(launch/map_merge.launch)
) # roslaunch_add_file_check(launch/experiments)
execute_process( # endif()
COMMAND bash download.sh ${CMAKE_BINARY_DIR}/map05.pgm ${base_url}/hector_maps/map05.pgm cb9154c9fa3d97e5e992592daca9853a
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/test
)
execute_process(
COMMAND bash download.sh ${CMAKE_BINARY_DIR}/2011-08-09-12-22-52.pgm ${base_url}/gmapping_maps/2011-08-09-12-22-52.pgm 3c2c38e7dec2b7a67f41069ab58badaa
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/test
)
execute_process(
COMMAND bash download.sh ${CMAKE_BINARY_DIR}/2012-01-28-11-12-01.pgm ${base_url}/gmapping_maps/2012-01-28-11-12-01.pgm 681e704044889c95e47b0c3aadd81f1e
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/test
)
ament_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})
endif()
ament_export_include_directories(include)
ament_export_libraries(combine_grids)
ament_package()

2
map_merge/config/params.yaml
View File

@ -5,7 +5,7 @@ map_merge:
estimation_rate: 0.5 estimation_rate: 0.5
estimation_confidence: 0.6 estimation_confidence: 0.6
robot_map_topic: map robot_map_topic: map
robot_map_updates_topic: map_updates robot_map_updates_topic: map_udpates
robot_namespace: "" robot_namespace: ""
merged_map_topic: map merged_map_topic: map
world_frame: world world_frame: world

71
map_merge/launch/tb3_simulation/multi_tb3_simulation_launch.py
View File

@ -23,6 +23,7 @@ The robots co-exist on a shared environment and are controlled by independent na
import os import os
from ament_index_python.packages import get_package_share_directory from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription, condition from launch import LaunchDescription, condition
from launch.actions import ( from launch.actions import (
DeclareLaunchArgument, DeclareLaunchArgument,
@ -34,7 +35,6 @@ from launch.actions import (
from launch.conditions import IfCondition, UnlessCondition from launch.conditions import IfCondition, UnlessCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, TextSubstitution from launch.substitutions import LaunchConfiguration, TextSubstitution
from launch_ros.actions import Node
def generate_launch_description(): def generate_launch_description():
@ -51,7 +51,7 @@ def generate_launch_description():
{"name": "robot1", "x_pose": 0.0, "y_pose": 0.5, "z_pose": 0.01}, {"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}, {"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 beginning # Names and poses of the robots for unknown poses demo, the must be very close at beggining
robots_unknown_poses = [ robots_unknown_poses = [
{"name": "robot1", "x_pose": -2.0, "y_pose": 0.5, "z_pose": 0.01}, {"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}, {"name": "robot2", "x_pose": -3.0, "y_pose": 0.5, "z_pose": 0.01},
@ -159,75 +159,9 @@ def generate_launch_description():
output="screen", output="screen",
) )
robot_sdf = LaunchConfiguration("robot_sdf")
declare_robot_sdf_cmd = DeclareLaunchArgument(
"robot_sdf",
default_value=os.path.join(bringup_dir, "worlds", "waffle.model"),
description="Full path to robot sdf file to spawn the robot in gazebo",
)
# Define commands for spawing the robots into Gazebo # Define commands for spawing the robots into Gazebo
spawn_robots_cmds = [] spawn_robots_cmds = []
for robot_known, robot_unknown in zip(robots_known_poses, robots_unknown_poses): for robot_known, robot_unknown in zip(robots_known_poses, robots_unknown_poses):
# after humble release, use spawn_entity.py
if os.getenv("ROS_DISTRO") == "humble":
spawn_robots_cmds.append(
Node(
package="gazebo_ros",
executable="spawn_entity.py",
output="screen",
arguments=[
"-entity",
robot_known["name"],
"-file",
robot_sdf,
"-robot_namespace",
TextSubstitution(text=str(robot_known["name"])),
"-x",
TextSubstitution(text=str(robot_known["x_pose"])),
"-y",
TextSubstitution(text=str(robot_known["y_pose"])),
"-z",
TextSubstitution(text=str(robot_known["z_pose"])),
"-R",
"0.0",
"-P",
"0.0",
"-Y",
"0.0",
],
condition=IfCondition(known_init_poses),
)
)
spawn_robots_cmds.append(
Node(
package="gazebo_ros",
executable="spawn_entity.py",
output="screen",
arguments=[
"-entity",
robot_unknown["name"],
"-file",
robot_sdf,
"-robot_namespace",
TextSubstitution(text=str(robot_unknown["name"])),
"-x",
TextSubstitution(text=str(robot_unknown["x_pose"])),
"-y",
TextSubstitution(text=str(robot_unknown["y_pose"])),
"-z",
TextSubstitution(text=str(robot_unknown["z_pose"])),
"-R",
"0.0",
"-P",
"0.0",
"-Y",
"0.0",
],
condition=UnlessCondition(known_init_poses),
)
)
else:
spawn_robots_cmds.append( spawn_robots_cmds.append(
IncludeLaunchDescription( IncludeLaunchDescription(
PythonLaunchDescriptionSource( PythonLaunchDescriptionSource(
@ -346,7 +280,6 @@ def generate_launch_description():
ld.add_action(declare_slam_toolbox_cmd) ld.add_action(declare_slam_toolbox_cmd)
ld.add_action(declare_slam_gmapping_cmd) ld.add_action(declare_slam_gmapping_cmd)
ld.add_action(declare_known_init_poses_cmd) ld.add_action(declare_known_init_poses_cmd)
ld.add_action(declare_robot_sdf_cmd)
# Add the actions to start gazebo, robots and simulations # Add the actions to start gazebo, robots and simulations
ld.add_action(start_gazebo_cmd) ld.add_action(start_gazebo_cmd)

2
map_merge/package.xml
View File

@ -20,8 +20,6 @@
<depend>nav_msgs</depend> <depend>nav_msgs</depend>
<depend>map_msgs</depend> <depend>map_msgs</depend>
<depend>tf2_geometry_msgs</depend> <depend>tf2_geometry_msgs</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<!-- used to get OpenCV --> <!-- used to get OpenCV -->
<depend>image_geometry</depend> <depend>image_geometry</depend>

33
map_merge/src/combine_grids/merging_pipeline.cpp
View File

@ -101,27 +101,28 @@ bool MergingPipeline::estimateTransforms(FeatureType feature_type,
// no match found. try set first non-empty grid as reference frame. we try to // 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 // avoid setting empty grid as reference frame, in case some maps never
// arrive. If all is empty just set null transforms. // arrive. If all is empty just set null transforms.
if (good_indices.size() == 1) { // if (good_indices.size() == 1) {
transforms_.clear();
transforms_.resize(images_.size());
// Making some tests to see if it is better to just return false if no match is found
// and not clear the last good transforms found
// if (images_.size() != transforms_.size()) {
// transforms_.clear(); // transforms_.clear();
// transforms_.resize(images_.size()); // 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;
// } // }
// return false;
for (size_t i = 0; i < images_.size(); ++i) { // Making some tests it is better to just return false if no match is found
if (!images_[i].empty()) { // and not clear the last good transforms found
// set identity if (good_indices.size() == 1) {
transforms_[i] = cv::Mat::eye(3, 3, CV_64F); if (images_.size() != transforms_.size()) {
break; transforms_.clear();
transforms_.resize(images_.size());
} }
} return false;
// RCLCPP_INFO(logger, "No match found between maps, setting first non-empty grid as reference frame");
return true;
} }
// // Experimental: should we keep only the best confidence match overall? // // Experimental: should we keep only the best confidence match overall?

21
map_merge/test/download.sh
View File

@ -1,21 +0,0 @@
file_name=$1
url=$2
md5=$3
# Download the file if it doesn't exist
if [ ! -f $file_name ]; then
wget $url -O $file_name
fi
# Check the MD5 sum of the file
echo "Checking MD5 sum of $file_name"
md5sum -c <<<"$md5 $file_name"
if [ $? -ne 0 ]; then
echo "MD5 sum of $file_name does not match. Downloading it again"
wget $url -O $file_name
md5sum -c <<<"$md5 $file_name"
if [ $? -ne 0 ]; then
echo "MD5 sum of $file_name still does not match. Aborting."
exit 1
fi
fi

5
map_merge/test/download_data.sh
View File

@ -1,5 +0,0 @@
base_url=https://raw.githubusercontent.com/hrnr/m-explore-extra/master/map_merge
wget ${base_url}/gmapping_maps/2012-01-28-11-12-01.pgm -P build/multirobot_map_merge
wget ${base_url}/gmapping_maps/2011-08-09-12-22-52.pgm -P build/multirobot_map_merge
wget ${base_url}/hector_maps/map05.pgm -P build/multirobot_map_merge
wget ${base_url}/hector_maps/map00.pgm -P build/multirobot_map_merge

49
map_merge/test/test_merging_pipeline.cpp
View File

@ -3,7 +3,6 @@
* Software License Agreement (BSD License) * Software License Agreement (BSD License)
* *
* Copyright (c) 2015-2016, Jiri Horner. * Copyright (c) 2015-2016, Jiri Horner.
* Copyright (c) 2022, Carlos Alvarez.
* All rights reserved. * All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
@ -37,7 +36,8 @@
#include <combine_grids/grid_warper.h> #include <combine_grids/grid_warper.h>
#include <gtest/gtest.h> #include <gtest/gtest.h>
// #include <ros/console.h> #include <ros/console.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <opencv2/core/utility.hpp> #include <opencv2/core/utility.hpp>
#include "testing_helpers.h" #include "testing_helpers.h"
@ -64,12 +64,12 @@ constexpr bool verbose_tests = false;
TEST(MergingPipeline, canStich0Grid) TEST(MergingPipeline, canStich0Grid)
{ {
std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> maps; std::vector<nav_msgs::OccupancyGridConstPtr> maps;
combine_grids::MergingPipeline merger; combine_grids::MergingPipeline merger;
merger.feed(maps.begin(), maps.end()); merger.feed(maps.begin(), maps.end());
EXPECT_TRUE(merger.estimateTransforms()); EXPECT_TRUE(merger.estimateTransforms());
EXPECT_EQ(merger.composeGrids(), nullptr); EXPECT_EQ(merger.composeGrids(), nullptr);
EXPECT_EQ(merger.getTransforms().size(), (long unsigned int) 0); EXPECT_EQ(merger.getTransforms().size(), 0);
} }
TEST(MergingPipeline, canStich1Grid) TEST(MergingPipeline, canStich1Grid)
@ -82,11 +82,10 @@ TEST(MergingPipeline, canStich1Grid)
EXPECT_VALID_GRID(merged_grid); EXPECT_VALID_GRID(merged_grid);
// don't use EXPECT_EQ, since it prints too much info // don't use EXPECT_EQ, since it prints too much info
EXPECT_TRUE(maps_equal(*merged_grid, *map)); EXPECT_TRUE(*merged_grid == *map);
// check estimated transforms // check estimated transforms
auto transforms = merger.getTransforms(); auto transforms = merger.getTransforms();
EXPECT_EQ(transforms.size(), (long unsigned int) 1); EXPECT_EQ(transforms.size(), 1);
EXPECT_TRUE(isIdentity(transforms[0])); EXPECT_TRUE(isIdentity(transforms[0]));
} }
@ -157,7 +156,7 @@ TEST(MergingPipeline, estimationAccuracy)
EXPECT_VALID_GRID(merged_grid); EXPECT_VALID_GRID(merged_grid);
// transforms // transforms
auto transforms = merger.getTransforms(); auto transforms = merger.getTransforms();
EXPECT_EQ(transforms.size(), (long unsigned int) 2); EXPECT_EQ(transforms.size(), 2);
EXPECT_TRUE(isIdentity(transforms[0])); EXPECT_TRUE(isIdentity(transforms[0]));
tf2::Transform t; tf2::Transform t;
tf2::fromMsg(transforms[1], t); tf2::fromMsg(transforms[1], t);
@ -177,7 +176,7 @@ TEST(MergingPipeline, transformsRoundTrip)
merger.setTransforms(&in_transform, &in_transform + 1); merger.setTransforms(&in_transform, &in_transform + 1);
auto out_transforms = merger.getTransforms(); auto out_transforms = merger.getTransforms();
ASSERT_EQ(out_transforms.size(), (long unsigned int) 1); ASSERT_EQ(out_transforms.size(), 1);
auto out_transform = out_transforms[0]; auto out_transform = out_transforms[0];
EXPECT_FLOAT_EQ(in_transform.translation.x, out_transform.translation.x); 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.y, out_transform.translation.y);
@ -199,7 +198,7 @@ TEST(MergingPipeline, setTransformsInternal)
auto transform = randomTransform(); auto transform = randomTransform();
merger.setTransforms(&transform, &transform + 1); merger.setTransforms(&transform, &transform + 1);
ASSERT_EQ(merger.transforms_.size(), (long unsigned int) 1); ASSERT_EQ(merger.transforms_.size(), 1);
auto& transform_internal = merger.transforms_[0]; auto& transform_internal = merger.transforms_[0];
// verify that transforms are the same in 2D // verify that transforms are the same in 2D
tf2::Vector3 a[2] = {{1., 0., 1.}, {0., 1., 1.}}; tf2::Vector3 a[2] = {{1., 0., 1.}, {0., 1., 1.}};
@ -229,7 +228,7 @@ TEST(MergingPipeline, getTransformsInternal)
cv::Mat transform_internal = randomTransformMatrix(); cv::Mat transform_internal = randomTransformMatrix();
merger.transforms_[0] = transform_internal; merger.transforms_[0] = transform_internal;
auto transforms = merger.getTransforms(); auto transforms = merger.getTransforms();
ASSERT_EQ(transforms.size(), (long unsigned int) 1); ASSERT_EQ(transforms.size(), 1);
// output quaternion should be normalized // output quaternion should be normalized
auto& q = transforms[0].rotation; 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); EXPECT_DOUBLE_EQ(1., q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
@ -251,8 +250,8 @@ TEST(MergingPipeline, getTransformsInternal)
TEST(MergingPipeline, setEmptyTransforms) TEST(MergingPipeline, setEmptyTransforms)
{ {
constexpr size_t size = 2; constexpr size_t size = 2;
std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> maps(size); std::vector<nav_msgs::OccupancyGridConstPtr> maps(size);
std::vector<geometry_msgs::msg::Transform> transforms(size); std::vector<geometry_msgs::Transform> transforms(size);
combine_grids::MergingPipeline merger; combine_grids::MergingPipeline merger;
merger.feed(maps.begin(), maps.end()); merger.feed(maps.begin(), maps.end());
merger.setTransforms(transforms.begin(), transforms.end()); merger.setTransforms(transforms.begin(), transforms.end());
@ -264,8 +263,8 @@ TEST(MergingPipeline, setEmptyTransforms)
TEST(MergingPipeline, emptyImageWithTransform) TEST(MergingPipeline, emptyImageWithTransform)
{ {
constexpr size_t size = 1; constexpr size_t size = 1;
std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> maps(size); std::vector<nav_msgs::OccupancyGridConstPtr> maps(size);
std::vector<geometry_msgs::msg::Transform> transforms(size); std::vector<geometry_msgs::Transform> transforms(size);
transforms[0].rotation.z = 1; // set transform to identity transforms[0].rotation.z = 1; // set transform to identity
combine_grids::MergingPipeline merger; combine_grids::MergingPipeline merger;
merger.feed(maps.begin(), maps.end()); merger.feed(maps.begin(), maps.end());
@ -277,7 +276,7 @@ TEST(MergingPipeline, emptyImageWithTransform)
/* one image may be empty */ /* one image may be empty */
TEST(MergingPipeline, oneEmptyImage) TEST(MergingPipeline, oneEmptyImage)
{ {
std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> maps{nullptr, std::vector<nav_msgs::OccupancyGridConstPtr> maps{nullptr,
loadMap(gmapping_maps[0])}; loadMap(gmapping_maps[0])};
combine_grids::MergingPipeline merger; combine_grids::MergingPipeline merger;
merger.feed(maps.begin(), maps.end()); merger.feed(maps.begin(), maps.end());
@ -287,9 +286,9 @@ TEST(MergingPipeline, oneEmptyImage)
EXPECT_VALID_GRID(merged_grid); EXPECT_VALID_GRID(merged_grid);
// don't use EXPECT_EQ, since it prints too much info // don't use EXPECT_EQ, since it prints too much info
EXPECT_TRUE(maps_equal(*merged_grid, *maps[1])); EXPECT_TRUE(*merged_grid == *maps[1]);
// transforms // transforms
EXPECT_EQ(transforms.size(), (long unsigned int) 2); EXPECT_EQ(transforms.size(), 2);
EXPECT_TRUE(isIdentity(transforms[1])); EXPECT_TRUE(isIdentity(transforms[1]));
} }
@ -301,7 +300,7 @@ TEST(MergingPipeline, knownInitPositions)
merger.feed(maps.begin(), maps.end()); merger.feed(maps.begin(), maps.end());
for (size_t i = 0; i < 5; ++i) { for (size_t i = 0; i < 5; ++i) {
std::vector<geometry_msgs::msg::Transform> transforms{randomTransform(), std::vector<geometry_msgs::Transform> transforms{randomTransform(),
randomTransform()}; randomTransform()};
merger.setTransforms(transforms.begin(), transforms.end()); merger.setTransforms(transforms.begin(), transforms.end());
auto merged_grid = merger.composeGrids(); auto merged_grid = merger.composeGrids();
@ -312,12 +311,12 @@ TEST(MergingPipeline, knownInitPositions)
int main(int argc, char** argv) int main(int argc, char** argv)
{ {
// ros::Time::init(); ros::Time::init();
// if (verbose_tests && if (verbose_tests &&
// ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
// ros::console::levels::Debug)) { ros::console::levels::Debug)) {
// ros::console::notifyLoggerLevelsChanged(); ros::console::notifyLoggerLevelsChanged();
// } }
testing::InitGoogleTest(&argc, argv); testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS(); return RUN_ALL_TESTS();
} }

52
map_merge/test/testing_helpers.h
View File

@ -1,33 +1,28 @@
#ifndef TESTING_HELPERS_H_ #ifndef TESTING_HELPERS_H_
#define TESTING_HELPERS_H_ #define TESTING_HELPERS_H_
#include <nav_msgs/msg/occupancy_grid.hpp> #include <nav_msgs/OccupancyGrid.h>
#include <geometry_msgs/msg/transform.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h> #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2/convert.h>
#include <opencv2/core/utility.hpp> #include <opencv2/core/utility.hpp>
#include <opencv2/imgcodecs.hpp> #include <opencv2/imgcodecs.hpp>
#include <random> #include <random>
const float resolution = 0.05f; const float resolution = 0.05f;
nav_msgs::msg::OccupancyGrid::ConstSharedPtr loadMap(const std::string& filename); nav_msgs::OccupancyGridConstPtr loadMap(const std::string& filename);
void saveMap(const std::string& filename, void saveMap(const std::string& filename,
const nav_msgs::msg::OccupancyGrid::ConstSharedPtr& map); const nav_msgs::OccupancyGridConstPtr& map);
std::tuple<double, double, double> randomAngleTxTy(); std::tuple<double, double, double> randomAngleTxTy();
geometry_msgs::msg::Transform randomTransform(); geometry_msgs::Transform randomTransform();
cv::Mat randomTransformMatrix(); cv::Mat randomTransformMatrix();
/* map_server is really bad. until there is no replacement I will implement it /* map_server is really bad. until there is no replacement I will implement it
* by myself */ * by myself */
template <typename InputIt> template <typename InputIt>
std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> loadMaps(InputIt filenames_begin, std::vector<nav_msgs::OccupancyGridConstPtr> loadMaps(InputIt filenames_begin,
InputIt filenames_end) InputIt filenames_end)
{ {
std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> result; std::vector<nav_msgs::OccupancyGridConstPtr> result;
for (InputIt it = filenames_begin; it != filenames_end; ++it) { for (InputIt it = filenames_begin; it != filenames_end; ++it) {
result.emplace_back(loadMap(*it)); result.emplace_back(loadMap(*it));
@ -35,7 +30,7 @@ std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> loadMaps(InputIt filen
return result; return result;
} }
nav_msgs::msg::OccupancyGrid::ConstSharedPtr loadMap(const std::string& filename) nav_msgs::OccupancyGridConstPtr loadMap(const std::string& filename)
{ {
cv::Mat lookUpTable(1, 256, CV_8S); cv::Mat lookUpTable(1, 256, CV_8S);
signed char* p = lookUpTable.ptr<signed char>(); signed char* p = lookUpTable.ptr<signed char>();
@ -47,7 +42,7 @@ nav_msgs::msg::OccupancyGrid::ConstSharedPtr loadMap(const std::string& filename
if (img.empty()) { if (img.empty()) {
throw std::runtime_error("could not load map"); throw std::runtime_error("could not load map");
} }
nav_msgs::msg::OccupancyGrid::SharedPtr grid{new nav_msgs::msg::OccupancyGrid()}; nav_msgs::OccupancyGridPtr grid{new nav_msgs::OccupancyGrid()};
grid->info.width = static_cast<uint>(img.size().width); grid->info.width = static_cast<uint>(img.size().width);
grid->info.height = static_cast<uint>(img.size().height); grid->info.height = static_cast<uint>(img.size().height);
grid->info.resolution = resolution; grid->info.resolution = resolution;
@ -60,7 +55,7 @@ nav_msgs::msg::OccupancyGrid::ConstSharedPtr loadMap(const std::string& filename
} }
void saveMap(const std::string& filename, void saveMap(const std::string& filename,
const nav_msgs::msg::OccupancyGrid::ConstSharedPtr& map) const nav_msgs::OccupancyGridConstPtr& map)
{ {
cv::Mat lookUpTable(1, 256, CV_8U); cv::Mat lookUpTable(1, 256, CV_8U);
uchar* p = lookUpTable.ptr(); uchar* p = lookUpTable.ptr();
@ -89,7 +84,7 @@ std::tuple<double, double, double> randomAngleTxTy()
translation_dis(g)); translation_dis(g));
} }
geometry_msgs::msg::Transform randomTransform() geometry_msgs::Transform randomTransform()
{ {
double angle, tx, ty; double angle, tx, ty;
std::tie(angle, tx, ty) = randomAngleTxTy(); std::tie(angle, tx, ty) = randomAngleTxTy();
@ -124,14 +119,14 @@ cv::Mat randomTransformMatrix()
return transform; return transform;
} }
static inline bool isIdentity(const geometry_msgs::msg::Transform& transform) static inline bool isIdentity(const geometry_msgs::Transform& transform)
{ {
tf2::Transform t; tf2::Transform t;
tf2::fromMsg(transform, t); tf2::fromMsg(transform, t);
return tf2::Transform::getIdentity() == t; return tf2::Transform::getIdentity() == t;
} }
static inline bool isIdentity(const geometry_msgs::msg::Quaternion& rotation) static inline bool isIdentity(const geometry_msgs::Quaternion& rotation)
{ {
tf2::Quaternion q; tf2::Quaternion q;
tf2::fromMsg(rotation, q); tf2::fromMsg(rotation, q);
@ -139,32 +134,15 @@ static inline bool isIdentity(const geometry_msgs::msg::Quaternion& rotation)
} }
// data size is consistent with height and width // data size is consistent with height and width
static inline bool consistentData(const nav_msgs::msg::OccupancyGrid& grid) static inline bool consistentData(const nav_msgs::OccupancyGrid& grid)
{ {
return grid.info.width * grid.info.height == grid.data.size(); return grid.info.width * grid.info.height == grid.data.size();
} }
// ignores header, map_load_time and origin // ignores header, map_load_time and origin
// static inline bool operator==(const nav_msgs::msg::OccupancyGrid::SharedPtr grid1, static inline bool operator==(const nav_msgs::OccupancyGrid& grid1,
// const nav_msgs::msg::OccupancyGrid::SharedPtr grid2) 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;
// }
// ignores header, map_load_time and origin
static inline bool maps_equal(const nav_msgs::msg::OccupancyGrid& grid1,
const nav_msgs::msg::OccupancyGrid& grid2)
{ {
// std::cout << "asdasdadsdth: " << std::endl;
bool equal = true; bool equal = true;
equal &= grid1.info.width == grid2.info.width; equal &= grid1.info.width == grid2.info.width;
equal &= grid1.info.height == grid2.info.height; equal &= grid1.info.height == grid2.info.height;