diff --git a/README.md b/README.md index b14752e..3f6b679 100644 --- a/README.md +++ b/README.md @@ -1,18 +1,86 @@ -# 开始拉取代码 +# 基于ROS2的无人小车自主探索与建图 +本卡片让机器人在未知办公室环境中自主探索，并生成 2D 栅格地图。当机器人还不知道环境长什么样，无人小车通过本模块知道如何先把环境“走一遍、画出来”。本卡片依赖ROS2 Humble、Nav2、TurtleBot3 Gazebo、SLAM Toolbox 与 explore_lite 完成自主探索与地图保存。 + + +## 1. 环境要求 +- Ubuntu 22.04 +- ROS2 Humble +- Gazebo Sim Harmonic --- -``` -git clone http://git-test.databall.tech:3000/hq/ros2_office_mapping.git -cd ros2_office_mapping +### 编译 -# add your files to repo -git push +```bash +cd /workspace +colcon build --symlink-install +source /opt/ros/humble/setup.bash +source install/setup.bash ``` ---- + +### 确认 package 能被 ROS2 找到 + +```bash +ros2 pkg list | grep explore ``` -cd existing_repo -git remote add origin http://git-test.databall.tech:3000/hq/ros2_office_mapping.git -git branch -M main -git push -uf origin main -``` \ No newline at end of file +预计打印结果：explore_lite + +### 注意事项 + +之后每打开一个新终端，都建议先执行： + +```bash +source /opt/ros/humble/setup.bash +source install/setup.bash +export TURTLEBOT3_MODEL=burger +``` + +为了方便，也可以写入 `~/.bashrc`： + +```bash +echo 'source /opt/ros/humble/setup.bash' >> ~/.bashrc +echo 'source /workspace/install/setup.bash' >> ~/.bashrc +echo 'export TURTLEBOT3_MODEL=burger' >> ~/.bashrc +source ~/.bashrc +``` + +## 2. 启动仿真， Nav2导航栈， SLAM Toolbox异步建图， rviz2 + +```bash +./gzsim_run.sh + +ros2 launch slam_toolbox online_async_launch.py use_sim_time:=True + +ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True headless:=False + +ros2 run rviz2 rviz2 -d /opt/ros/humble/share/nav2_bringup/rviz/nav2_default_view.rviz --ros-args -p use_sim_time:=true + +``` + +## 3. 启动explore_lite 自主探索节点（如果机器人在探索时在某一点长时间卡住，可以在rviz中使用Nav2 Goal进行单点导航探索。） +```bash +ros2 launch explore_lite explore.launch.py +``` + +![图1](assets/picture01.png) + +## 4. 保存建图结果 + +机器人完成环境全区域探索后，新开终端执行地图保存命令，地图将保存至$NAV2_MAP_PATH指定路径 + +```bash +export NAV2_MAP_PATH=/workspace/src/turtlebot3_simulations/turtlebot3_gazebo/map +ros2 run nav2_map_server map_saver_cli -f ${NAV2_MAP_PATH}/office_map +``` + +## 5. 模型说明 +该package 基于explore_lite进行 ROS2 适配与二次开发，是未知环境自主建图的核心，实现地图前沿检测、探索目标。m-explore-ros2 package 的src/目录下包含explore.cpp、costmap_client.cpp、frontier_search.cpp，分别实现探索节点主要逻辑、代价地图数据subscribe与同步， frontier_search的核心算法。 + +* costmap_client.cpp 管理代价地图的数据，核心class 是 explore::Costmap2DClient， 实现了代价地图订阅、格式转换、位姿计算的具体逻辑，为上层提供了线程安全的代价地图数据和机器人实时位姿。这层的作用：给探索模块提供“实时可查询的栅格地图 + 当前机器人位姿”。首先启动时订阅 map 和 map_updates，把 OccupancyGrid 转成 nav2_costmap_2d::Costmap2D（updateFullMap / updatePartialMap）。其次，getRobotPose() 通过 TF 把 robot_base_frame 转到地图坐标系，给后续前沿搜索当起点。 + +* frontier_search.cpp 实现了前沿检测与优先级排序算法，核心的class 是frontier_exploration::FrontierSearch， 实现了 BFS 前沿检测、前沿属性计算、成本排序的核心逻辑，接收 Costmap2DClient 提供的代价地图，输出按优先级排序的前沿列表。具体来说，searchFrom(position) 从机器人附近 free cell 开始做 BFS，然后把“未知栅格且4邻域有free”的格子判为 frontier cell（isNewFrontierCell），把“未知栅格且4邻域有free”的格子判为 frontier cell（isNewFrontierCell），用 8 邻域把相连 frontier cell 聚成一个 frontier（buildNewFrontier），并计算size（前沿大小）、centroid（几何中心）、min_distance（离机器人最近点距离）。最后按代价进行排序，cost = potential_scale * distance - gain_scale * size +即“近的更好、大的更好（代价更低）”。 + +* explore.cpp：探索节点主逻辑，核心的class是explore::Explore，节点继承自 rclcpp::Node，是 ROS2 探索节点的入口类，在主循环中实现了导航调用、状态控制的流程。具体来说，Explore 节点创建 Nav2 action client（navigate_to_pose），定时器周期调用 makePlan()。makePlan() 核心逻辑是：拿机器人位姿->调 search_.searchFrom() 找 frontiers->过滤 blacklist->选代价最低目标并发给 Nav2->若长时间没进展（progress_timeout），把目标加入 blacklist 并重规划。reachedGoal() 根据 action 结果更新。stop/resume 通过 explore/resume topic 控制暂停恢复。可选 return_to_init=true 时，探索结束回初始点。 + +* launch/explore.launch.py，启动的入口，声明并加载所有配置参数（ planner_frequency、potential_scale、min_frontier_size 等），直接传递给 Explore 节点的构造函数 diff --git a/assets/picture01.png b/assets/picture01.png new file mode 100644 index 0000000..d9973e1 Binary files /dev/null and b/assets/picture01.png differ diff --git a/gzsim_run.sh b/gzsim_run.sh new file mode 100755 index 0000000..e0a23a6 --- /dev/null +++ b/gzsim_run.sh @@ -0,0 +1,68 @@ +#!/usr/bin/env bash +set -euo pipefail + + +ROOT="/workspace" + +sleep 5 + +set +u +source /opt/ros/humble/setup.bash +source "${ROOT}/install/setup.bash" +set -u + +LOG_DIR="${ROOT}/autorun_logs/autorun_$(date +%Y%m%d_%H%M%S)" +mkdir -p "$LOG_DIR" + +kill_port_listeners() { + local port="$1" + local pids + pids="$(ss -ltnp 2>/dev/null | grep ":${port} " | sed -E 's/.*pid=([0-9]+).*/\1/' | sort -u || true)" + if [[ -n "${pids}" ]]; then + echo "[run_all] port ${port} already in use, killing: ${pids}" + kill ${pids} 2>/dev/null || true + sleep 1 + fi +} + +kill_port_listeners 9002 + +export TURTLEBOT3_MODEL=burger +ros2 launch turtlebot3_gazebo turtlebot3_office.launch.py > "${LOG_DIR}/tb3_gzsim.log" 2>&1 & +GZSIM_PID=$! +# gz sim -v 4 -s -r empty.sdf > "${LOG_DIR}/empty_gzsim.log" 2>&1 & +# GZSIM_PID=$! + +echo "[autorun] tb3_gzsim pid=${GZSIM_PID}, log=${LOG_DIR}/tb3_gzsim.log" + +sleep 8 + +if ps -p "$GZSIM_PID" > /dev/null; then + echo "[autorun] tb3_gzsim is still running" +else + echo "[autorun] tb3_gzsim exited early. Log:" + cat "${LOG_DIR}/tb3_gzsim.log" +fi + +echo "[autorun] starting websocket ..." + +gz launch "${ROOT}/src/websocket.gzlaunch" > "${LOG_DIR}/websocket.log" 2>&1 & +WEBSOCKET_PID=$! + +echo "[autorun] websocket pid=${WEBSOCKET_PID}, log=${LOG_DIR}/websocket.log" + +sleep 10 + +if ps -p "$WEBSOCKET_PID" > /dev/null; then + echo "[autorun] websocket is still running" +else + echo "[autorun] websocket exited early. Log:" + cat "${LOG_DIR}/websocket.log" + exit 1 +fi + +echo "[autorun] all services started." +echo "[autorun] tb3_gzsim pid=${GZSIM_PID}" +echo "[autorun] websocket pid=${WEBSOCKET_PID}" + +wait diff --git a/src/m-explore-ros2/.gitignore b/src/m-explore-ros2/.gitignore new file mode 100755 index 0000000..e80f431 --- /dev/null +++ b/src/m-explore-ros2/.gitignore @@ -0,0 +1,3 @@ +*sublime-* +*.svg +*.xcf diff --git a/src/m-explore-ros2/LICENSE b/src/m-explore-ros2/LICENSE new file mode 100755 index 0000000..35366cf --- /dev/null +++ b/src/m-explore-ros2/LICENSE @@ -0,0 +1,31 @@ +Software License Agreement (BSD License) + +Copyright (c) 2015-2016, Carlos Alvarez. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: + +* Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. +* Redistributions in binary form must reproduce the above + copyright notice, this list of conditions and the following + disclaimer in the documentation and/or other materials provided + with the distribution. +* Neither the name of the Carlos Alvarez nor the names of its + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. diff --git a/src/m-explore-ros2/README.md b/src/m-explore-ros2/README.md new file mode 100755 index 0000000..e69de29 diff --git a/src/m-explore-ros2/explore/.DS_Store b/src/m-explore-ros2/explore/.DS_Store new file mode 100644 index 0000000..8fe1481 Binary files /dev/null and b/src/m-explore-ros2/explore/.DS_Store differ diff --git a/src/m-explore-ros2/explore/CMakeLists.txt b/src/m-explore-ros2/explore/CMakeLists.txt new file mode 100755 index 0000000..1ebcfc6 --- /dev/null +++ b/src/m-explore-ros2/explore/CMakeLists.txt @@ -0,0 +1,121 @@ +cmake_minimum_required(VERSION 3.5) +project(explore_lite) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Set flag depending on distro +if(NOT DEFINED ENV{ROS_DISTRO}) + message(FATAL_ERROR "ROS_DISTRO is not defined." ) +endif() +if("$ENV{ROS_DISTRO}" STREQUAL "eloquent") + message(STATUS "Build for ROS2 eloquent") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DELOQUENT") +elseif("$ENV{ROS_DISTRO}" STREQUAL "dashing") + message(STATUS "Build for ROS2 dashing") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DDASHING") +else() + message(STATUS "BuilD for ROS2: " "$ENV{ROS_DISTRO}") +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(map_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(nav2_costmap_2d REQUIRED) + + +set(DEPENDENCIES + rclcpp + std_msgs + sensor_msgs + tf2 + tf2_ros + tf2_geometry_msgs + nav2_msgs + nav_msgs + map_msgs + nav2_costmap_2d + visualization_msgs +) + +include_directories( + include +) + +install( + DIRECTORY include/explore/ + DESTINATION include/explore/ +) + +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME} +) +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} +) + + +add_executable(explore + src/costmap_client.cpp + src/explore.cpp + src/frontier_search.cpp +) + +target_include_directories(explore PUBLIC + $ + $) + + +target_link_libraries(explore ${rclcpp_LIBRARIES}) + +ament_target_dependencies(explore ${DEPENDENCIES}) + +install(TARGETS explore + DESTINATION lib/${PROJECT_NAME}) + +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_package() \ No newline at end of file diff --git a/src/m-explore-ros2/explore/config/params.yaml b/src/m-explore-ros2/explore/config/params.yaml new file mode 100755 index 0000000..f8fa6a3 --- /dev/null +++ b/src/m-explore-ros2/explore/config/params.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + robot_base_frame: base_link + return_to_init: true + costmap_topic: map + costmap_updates_topic: map_updates + visualize: true + planner_frequency: 0.2 #0.15 + progress_timeout: 40.0 + potential_scale: 1.0 #3.0 + orientation_scale: 0.0 + gain_scale: 3.0 #1.0 + transform_tolerance: 0.3 + min_frontier_size: 0.5 #0.75 diff --git a/src/m-explore-ros2/explore/config/params_costmap.yaml b/src/m-explore-ros2/explore/config/params_costmap.yaml new file mode 100755 index 0000000..ddf29b4 --- /dev/null +++ b/src/m-explore-ros2/explore/config/params_costmap.yaml @@ -0,0 +1,13 @@ +explore_node: + ros__parameters: + robot_base_frame: base_link + costmap_topic: /global_costmap/costmap + costmap_updates_topic: /global_costmap/costmap_updates + visualize: true + planner_frequency: 0.2 + progress_timeout: 30.0 + potential_scale: 2.0 + orientation_scale: 0.0 + gain_scale: 3.0 + transform_tolerance: 0.3 + min_frontier_size: 0.3 diff --git a/src/m-explore-ros2/explore/doc/screenshot.png b/src/m-explore-ros2/explore/doc/screenshot.png new file mode 100755 index 0000000..216ade5 Binary files /dev/null and b/src/m-explore-ros2/explore/doc/screenshot.png differ diff --git a/src/m-explore-ros2/explore/doc/wiki_doc.txt b/src/m-explore-ros2/explore/doc/wiki_doc.txt new file mode 100755 index 0000000..5ddb4fe --- /dev/null +++ b/src/m-explore-ros2/explore/doc/wiki_doc.txt @@ -0,0 +1,148 @@ +<> + +<> + +<> + +== Overview == +This package provides greedy frontier-based exploration. When node is running, robot will greedily explore its environment until no frontiers could be found. Movement commands will be send to [[move_base]]. + +{{attachment:screenshot.png||width="755px"}} + +Unlike similar packages, {{{explore_lite}}} does not create its own costmap, which makes it easier to configure and more efficient (lighter on resources). Node simply subscribes to <> messages. Commands for robot movement are send to [[move_base]] node. + +Node can do frontier filtering and can operate even on non-inflated maps. Goal blacklisting allows to deal with places inaccessible for robot. + +<> + +== Architecture == +{{{explore_lite}}} uses [[move_base]] for navigation. You need to run properly configured [[move_base]] node. + +{{attachment:architecture.svg||width="755px"}} + +{{{explore_lite}}} subscribes to a <> and <> messages to construct a map where it looks for frontiers. You can either use costmap published by [[move_base]] (ie. `/global_costmap/costmap`) or you can use map constructed by mapping algorithm (SLAM). + +Depending on your environment you may achieve better results with either SLAM map or costmap published by `move_base`. Advantage of `move_base` costmap is the inflation which helps to deal with some very small unexplorable frontiers. When you are using a raw map produced by SLAM you should set the `min_frontier_size` parameter to some reasonable number to deal with the small frontiers. For details on both setups check the `explore.launch` and `explore_costmap.launch` launch files. + +== Setup == + +Before starting experimenting with {{{explore_lite}}} you need to have working [[move_base]] for navigation. You should be able to navigate with [[move_base]] manually through [[rviz]]. Please refer to [[navigation#Tutorials]] for setting up [[move_base]] and the rest of the navigation stack with your robot. + +You should be also able to to navigate with [[move_base]] though unknown space in the map. If you set the goal to unknown place in the map, planning and navigating should work. With most planners this should work by default, refer to [[navfn#Parameters]] if you need to setup this for [[navfn]] planner (but should be enabled by default). Navigation through unknown space is required for {{{explore_lite}}}. + +If you want to use costmap provided by [[move_base]] you need to enable unknown space tracking by setting `track_unknown_space: true`. + +If you have [[move_base]] configured correctly, you can start experimenting with {{{explore_lite}}}. Provided `explore.launch` should work out-of-the box in most cases, but as always you might need to adjust topic names and frame names according to your setup. + +== ROS API == +{{{ +#!clearsilver CS/NodeAPI + +name = explore +desc = Provides exploration services offered by this package. Exploration will start immediately after node initialization. + +pub { + 0.name = ~frontiers + 0.type = visualization_msgs/MarkerArray + 0.desc = Visualization of frontiers considered by exploring algorithm. Each frontier is visualized by frontier points in blue and with a small sphere, which visualize the cost of the frontiers (costlier frontiers will have smaller spheres). +} +sub { + 0.name = costmap + 0.type = nav_msgs/OccupancyGrid + 0.desc = Map which will be used for exploration planning. Can be either costmap from [[move_base]] or map created by SLAM (see above). Occupancy grid must have got properly marked unknown space, mapping algorithms usually track unknown space by default. If you want to use costmap provided by [[move_base]] you need to enable unknown space tracking by setting `track_unknown_space: true`. + + 1.name = costmap_updates + 1.type = map_msgs/OccupancyGridUpdate + 1.desc = Incremental updates on costmap. Not necessary if source of map is always publishing full updates, i.e. does not provide this topic. +} + +param { + 0.name = ~robot_base_frame + 0.default = `base_link` + 0.type = string + 0.desc = The name of the base frame of the robot. This is used for determining robot position on map. Mandatory. + + 1.name = ~costmap_topic + 1.default = `costmap` + 1.type = string + 1.desc = Specifies topic of source <>. Mandatory. + + 3.name = ~costmap_updates_topic + 3.default = `costmap_updates` + 3.type = string + 3.desc = Specifies topic of source <>. Not necessary if source of map is always publishing full updates, i.e. does not provide this topic. + + 4.name = ~visualize + 4.default = `false` + 4.type = bool + 4.desc = Specifies whether or not publish visualized frontiers. + + 6.name = ~planner_frequency + 6.default = `1.0` + 6.type = double + 6.desc = Rate in Hz at which new frontiers will computed and goal reconsidered. + + 7.name = ~progress_timeout + 7.default = `30.0` + 7.type = double + 7.desc = Time in seconds. When robot do not make any progress for `progress_timeout`, current goal will be abandoned. + + 8.name = ~potential_scale + 8.default = `1e-3` + 8.type = double + 8.desc = Used for weighting frontiers. This multiplicative parameter affects frontier potential component of the frontier weight (distance to frontier). + + 9.name = ~orientation_scale + 9.default = `0` + 9.type = double + 9.desc = Used for weighting frontiers. This multiplicative parameter affects frontier orientation component of the frontier weight. This parameter does currently nothing and is provided solely for forward compatibility. + + 10.name = ~gain_scale + 10.default = `1.0` + 10.type = double + 10.desc = Used for weighting frontiers. This multiplicative parameter affects frontier gain component of the frontier weight (frontier size). + + 11.name = ~transform_tolerance + 11.default = `0.3` + 11.type = double + 11.desc = Transform tolerance to use when transforming robot pose. + + 12.name = ~min_frontier_size + 12.default = `0.5` + 12.type = double + 12.desc = Minimum size of the frontier to consider the frontier as the exploration goal. In meters. +} + +req_tf { + 0.from = global_frame + 0.to = robot_base_frame + 0.desc = This transformation is usually provided by mapping algorithm. Those frames are usually called `map` and `base_link`. For adjusting `robot_base_frame` name see respective parameter. You don't need to set `global_frame`. The name for `global_frame` will be sourced from `costmap_topic` automatically. +} + +act_called { + 0.name = move_base + 0.type = move_base_msgs/MoveBaseAction + 0.desc = [[move_base]] actionlib API for posting goals. See [[move_base#Action API]] for details. This expects [[move_base]] node in the same namespace as `explore_lite`, you may want to remap this node if this is not true. +} +}}} + +== Acknowledgements == + +This package was developed as part of my bachelor thesis at [[http://www.mff.cuni.cz/to.en/|Charles University]] in Prague. + +{{{ +@masterthesis{Hörner2016, + author = {Jiří Hörner}, + title = {Map-merging for multi-robot system}, + address = {Prague}, + year = {2016}, + school = {Charles University in Prague, Faculty of Mathematics and Physics}, + type = {Bachelor's thesis}, + URL = {https://is.cuni.cz/webapps/zzp/detail/174125/}, +} +}}} + +This project was initially based on [[explore]] package by Charles !DuHadway. Most of the node has been rewritten since then. The current frontier search algorithm is based on [[frontier_exploration]] by Paul Bovbel. + +## AUTOGENERATED DON'T DELETE +## CategoryPackage diff --git a/src/m-explore-ros2/explore/include/explore/costmap_client.h b/src/m-explore-ros2/explore/include/explore/costmap_client.h new file mode 100755 index 0000000..0adee31 --- /dev/null +++ b/src/m-explore-ros2/explore/include/explore/costmap_client.h @@ -0,0 +1,137 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2015-2016, Jiri Horner. + * Copyright (c) 2021, Carlos Alvarez, Juan Galvis. + * 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 COSTMAP_CLIENT_ +#define COSTMAP_CLIENT_ + +#include +#include + +#include +#include +#include +#include +#include + +#include "nav2_costmap_2d/costmap_2d_ros.hpp" + +namespace explore +{ +class Costmap2DClient +{ +public: + /** + * @brief Contructs client and start listening + * @details Constructor will block until first map update is received and + * map is ready to use, also will block before trasformation + * robot_base_frame <-> global_frame is available. + * + * @param node node handle to retrieve parameters from + * @param tf_listener Will be used for transformation of robot pose. + */ + Costmap2DClient(rclcpp::Node& node, const tf2_ros::Buffer* tf_listener); + /** + * @brief Get the pose of the robot in the global frame of the costmap + * @return pose of the robot in the global frame of the costmap + */ + geometry_msgs::msg::Pose getRobotPose() const; + + /** + * @brief Return a pointer to the "master" costmap which receives updates from + * all the layers. + * + * This pointer will stay the same for the lifetime of Costmap2DClient object. + */ + nav2_costmap_2d::Costmap2D* getCostmap() + { + return &costmap_; + } + + /** + * @brief Return a pointer to the "master" costmap which receives updates from + * all the layers. + * + * This pointer will stay the same for the lifetime of Costmap2DClient object. + */ + const nav2_costmap_2d::Costmap2D* getCostmap() const + { + return &costmap_; + } + + /** + * @brief Returns the global frame of the costmap + * @return The global frame of the costmap + */ + const std::string& getGlobalFrameID() const + { + return global_frame_; + } + + /** + * @brief Returns the local frame of the costmap + * @return The local frame of the costmap + */ + const std::string& getBaseFrameID() const + { + return robot_base_frame_; + } + +protected: + void updateFullMap(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); + void updatePartialMap(const map_msgs::msg::OccupancyGridUpdate::SharedPtr msg); + + nav2_costmap_2d::Costmap2D costmap_; + bool costmap_received_ = false; ///< @brief Flag indicating whether costmap + ///< callback has been called + + const tf2_ros::Buffer* const tf_; ///< @brief Used for transforming + /// point clouds + rclcpp::Node& node_; + std::string global_frame_; ///< @brief The global frame for the costmap + std::string robot_base_frame_; ///< @brief The frame_id of the robot base + double transform_tolerance_; ///< timeout before transform errors + +private: + // will be unsubscribed at destruction + rclcpp::Subscription::SharedPtr costmap_sub_; + rclcpp::Subscription::SharedPtr + costmap_updates_sub_; +}; + +} // namespace explore + +#endif diff --git a/src/m-explore-ros2/explore/include/explore/costmap_tools.h b/src/m-explore-ros2/explore/include/explore/costmap_tools.h new file mode 100755 index 0000000..d5d2fb4 --- /dev/null +++ b/src/m-explore-ros2/explore/include/explore/costmap_tools.h @@ -0,0 +1,135 @@ +#ifndef COSTMAP_TOOLS_H_ +#define COSTMAP_TOOLS_H_ + +#include +#include +#include + +#include "nav2_costmap_2d/costmap_2d_ros.hpp" + +namespace frontier_exploration +{ +/** + * @brief Determine 4-connected neighbourhood of an input cell, checking for map + * edges + * @param idx input cell index + * @param costmap Reference to map data + * @return neighbour cell indexes + */ +std::vector nhood4(unsigned int idx, + const nav2_costmap_2d::Costmap2D& costmap) +{ + // get 4-connected neighbourhood indexes, check for edge of map + std::vector out; + + unsigned int size_x_ = costmap.getSizeInCellsX(), + size_y_ = costmap.getSizeInCellsY(); + + if (idx > size_x_ * size_y_ - 1) { + RCLCPP_WARN(rclcpp::get_logger("FrontierExploration"), "Evaluating nhood " + "for offmap point"); + return out; + } + + if (idx % size_x_ > 0) { + out.push_back(idx - 1); + } + if (idx % size_x_ < size_x_ - 1) { + out.push_back(idx + 1); + } + if (idx >= size_x_) { + out.push_back(idx - size_x_); + } + if (idx < size_x_ * (size_y_ - 1)) { + out.push_back(idx + size_x_); + } + return out; +} + +/** + * @brief Determine 8-connected neighbourhood of an input cell, checking for map + * edges + * @param idx input cell index + * @param costmap Reference to map data + * @return neighbour cell indexes + */ +std::vector nhood8(unsigned int idx, + const nav2_costmap_2d::Costmap2D& costmap) +{ + // get 8-connected neighbourhood indexes, check for edge of map + std::vector out = nhood4(idx, costmap); + + unsigned int size_x_ = costmap.getSizeInCellsX(), + size_y_ = costmap.getSizeInCellsY(); + + if (idx > size_x_ * size_y_ - 1) { + return out; + } + + if (idx % size_x_ > 0 && idx >= size_x_) { + out.push_back(idx - 1 - size_x_); + } + if (idx % size_x_ > 0 && idx < size_x_ * (size_y_ - 1)) { + out.push_back(idx - 1 + size_x_); + } + if (idx % size_x_ < size_x_ - 1 && idx >= size_x_) { + out.push_back(idx + 1 - size_x_); + } + if (idx % size_x_ < size_x_ - 1 && idx < size_x_ * (size_y_ - 1)) { + out.push_back(idx + 1 + size_x_); + } + + return out; +} + +/** + * @brief Find nearest cell of a specified value + * @param result Index of located cell + * @param start Index initial cell to search from + * @param val Specified value to search for + * @param costmap Reference to map data + * @return True if a cell with the requested value was found + */ +bool nearestCell(unsigned int& result, unsigned int start, unsigned char val, + const nav2_costmap_2d::Costmap2D& costmap) +{ + const unsigned char* map = costmap.getCharMap(); + const unsigned int size_x = costmap.getSizeInCellsX(), + size_y = costmap.getSizeInCellsY(); + + if (start >= size_x * size_y) { + return false; + } + + // initialize breadth first search + std::queue bfs; + std::vector visited_flag(size_x * size_y, false); + + // push initial cell + bfs.push(start); + visited_flag[start] = true; + + // search for neighbouring cell matching value + while (!bfs.empty()) { + unsigned int idx = bfs.front(); + bfs.pop(); + + // return if cell of correct value is found + if (map[idx] == val) { + result = idx; + return true; + } + + // iterate over all adjacent unvisited cells + for (unsigned nbr : nhood8(idx, costmap)) { + if (!visited_flag[nbr]) { + bfs.push(nbr); + visited_flag[nbr] = true; + } + } + } + + return false; +} +} // namespace frontier_exploration +#endif diff --git a/src/m-explore-ros2/explore/include/explore/explore.h b/src/m-explore-ros2/explore/include/explore/explore.h new file mode 100755 index 0000000..bc39468 --- /dev/null +++ b/src/m-explore-ros2/explore/include/explore/explore.h @@ -0,0 +1,153 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Robert Bosch LLC. + * Copyright (c) 2015-2016, Jiri Horner. + * Copyright (c) 2021, Carlos Alvarez, Juan Galvis. + * 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 NAV_EXPLORE_H_ +#define NAV_EXPLORE_H_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "nav2_msgs/action/navigate_to_pose.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +using namespace std::placeholders; +#ifdef ELOQUENT +#define ACTION_NAME "NavigateToPose" +#elif DASHING +#define ACTION_NAME "NavigateToPose" +#else +#define ACTION_NAME "navigate_to_pose" +#endif +namespace explore +{ +/** + * @class Explore + * @brief A class adhering to the robot_actions::Action interface that moves the + * robot base to explore its environment. + */ +class Explore : public rclcpp::Node +{ +public: + Explore(); + ~Explore(); + + void start(); + void stop(bool finished_exploring = false); + void resume(); + void sendRandomGoal(); + + using NavigationGoalHandle = + rclcpp_action::ClientGoalHandle; + +private: + /** + * @brief Make a global plan + */ + + bool initial_random_exploration_; + size_t random_goal_index_; + std::vector random_goals_; + + + void makePlan(); + + // /** + // * @brief Publish a frontiers as markers + // */ + void visualizeFrontiers( + const std::vector& frontiers); + + bool goalOnBlacklist(const geometry_msgs::msg::Point& goal); + + NavigationGoalHandle::SharedPtr navigation_goal_handle_; + // void + // goal_response_callback(std::shared_future + // future); + void reachedGoal(const NavigationGoalHandle::WrappedResult& result, + const geometry_msgs::msg::Point& frontier_goal); + + rclcpp::Publisher::SharedPtr + marker_array_publisher_; + rclcpp::Logger logger_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + Costmap2DClient costmap_client_; + rclcpp_action::Client::SharedPtr + move_base_client_; + frontier_exploration::FrontierSearch search_; + rclcpp::TimerBase::SharedPtr exploring_timer_; + // rclcpp::TimerBase::SharedPtr oneshot_; + + rclcpp::Subscription::SharedPtr resume_subscription_; + void resumeCallback(const std_msgs::msg::Bool::SharedPtr msg); + + std::vector frontier_blacklist_; + geometry_msgs::msg::Point prev_goal_; + double prev_distance_; + rclcpp::Time last_progress_; + size_t last_markers_count_; + + geometry_msgs::msg::Pose initial_pose_; + void returnToInitialPose(void); + + // parameters + double planner_frequency_; + double potential_scale_, orientation_scale_, gain_scale_; + double progress_timeout_; + bool visualize_; + bool return_to_init_; + std::string robot_base_frame_; + bool resuming_ = false; +}; +} // namespace explore + +#endif diff --git a/src/m-explore-ros2/explore/include/explore/frontier_search.h b/src/m-explore-ros2/explore/include/explore/frontier_search.h new file mode 100755 index 0000000..0f8e610 --- /dev/null +++ b/src/m-explore-ros2/explore/include/explore/frontier_search.h @@ -0,0 +1,87 @@ +#ifndef FRONTIER_SEARCH_H_ +#define FRONTIER_SEARCH_H_ + +#include "nav2_costmap_2d/costmap_2d_ros.hpp" + +namespace frontier_exploration +{ +/** + * @brief Represents a frontier + * + */ +struct Frontier { + std::uint32_t size; + double min_distance; + double cost; + geometry_msgs::msg::Point initial; + geometry_msgs::msg::Point centroid; + geometry_msgs::msg::Point middle; + std::vector points; +}; + +/** + * @brief Thread-safe implementation of a frontier-search task for an input + * costmap. + */ +class FrontierSearch +{ +public: + FrontierSearch() : logger_(rclcpp::get_logger("frontier_search")) {} // Default constructor for the logger + + /** + * @brief Constructor for search task + * @param costmap Reference to costmap data to search. + */ + FrontierSearch(nav2_costmap_2d::Costmap2D* costmap, double potential_scale, + double gain_scale, double min_frontier_size, rclcpp::Logger logger); + + /** + * @brief Runs search implementation, outward from the start position + * @param position Initial position to search from + * @return List of frontiers, if any + */ + std::vector searchFrom(geometry_msgs::msg::Point position); + +protected: + /** + * @brief Starting from an initial cell, build a frontier from valid adjacent + * cells + * @param initial_cell Index of cell to start frontier building + * @param reference Reference index to calculate position from + * @param frontier_flag Flag vector indicating which cells are already marked + * as frontiers + * @return new frontier + */ + Frontier buildNewFrontier(unsigned int initial_cell, unsigned int reference, + std::vector& frontier_flag); + + /** + * @brief isNewFrontierCell Evaluate if candidate cell is a valid candidate + * for a new frontier. + * @param idx Index of candidate cell + * @param frontier_flag Flag vector indicating which cells are already marked + * as frontiers + * @return true if the cell is frontier cell + */ + bool isNewFrontierCell(unsigned int idx, + const std::vector& frontier_flag); + + /** + * @brief computes frontier cost + * @details cost function is defined by potential_scale and gain_scale + * + * @param frontier frontier for which compute the cost + * @return cost of the frontier + */ + double frontierCost(const Frontier& frontier); + +private: + nav2_costmap_2d::Costmap2D* costmap_; + unsigned char* map_; + unsigned int size_x_, size_y_; + double potential_scale_, gain_scale_; + double min_frontier_size_; + rclcpp::Logger logger_; +}; +} // namespace frontier_exploration +#endif diff --git a/src/m-explore-ros2/explore/launch/explore.launch.py b/src/m-explore-ros2/explore/launch/explore.launch.py new file mode 100755 index 0000000..5ae49ef --- /dev/null +++ b/src/m-explore-ros2/explore/launch/explore.launch.py @@ -0,0 +1,46 @@ +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("explore_lite"), "config", "params.yaml" + ) + use_sim_time = LaunchConfiguration("use_sim_time") + namespace = LaunchConfiguration("namespace") + + 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", + ) + + # 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="explore_lite", + name="explore_node", + namespace=namespace, + executable="explore", + parameters=[config, {"use_sim_time": use_sim_time}], + output="screen", + remappings=remappings, + ) + ld.add_action(declare_use_sim_time_argument) + ld.add_action(declare_namespace_argument) + ld.add_action(node) + return ld diff --git a/src/m-explore-ros2/explore/package.xml b/src/m-explore-ros2/explore/package.xml new file mode 100755 index 0000000..1653051 --- /dev/null +++ b/src/m-explore-ros2/explore/package.xml @@ -0,0 +1,33 @@ + + + + explore_lite + 1.0.0 + + Lightweight frontier-based exploration ROS2 port. + + echo + echo + BSD + + ament_cmake + + ament_lint_auto + ament_lint_common + ament_cmake + map_msgs + nav2_costmap_2d + nav2_msgs + nav_msgs + rclcpp + sensor_msgs + std_msgs + tf2 + tf2_geometry_msgs + tf2_ros + visualization_msgs + + + ament_cmake + + diff --git a/src/m-explore-ros2/explore/src/costmap_client.cpp b/src/m-explore-ros2/explore/src/costmap_client.cpp new file mode 100755 index 0000000..bab95c0 --- /dev/null +++ b/src/m-explore-ros2/explore/src/costmap_client.cpp @@ -0,0 +1,260 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2015-2016, Jiri Horner. + * Copyright (c) 2021, Carlos Alvarez, Juan Galvis. + * 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 +#include + +#include +#include +#include + +namespace explore +{ +std::array init_translation_table(); +static const std::array cost_translation_table__ = + init_translation_table(); + +Costmap2DClient::Costmap2DClient(rclcpp::Node& node, const tf2_ros::Buffer* tf) + : tf_(tf), node_(node) +{ + std::string costmap_topic; + std::string costmap_updates_topic; + + node_.declare_parameter("costmap_topic", std::string("costmap")); + node_.declare_parameter("costmap_updates_topic", + std::string("costmap_updates")); + node_.declare_parameter("robot_base_frame", std::string("base_" + "link")); + // transform tolerance is used for all tf transforms here + node_.declare_parameter("transform_tolerance", 0.3); + + node_.get_parameter("costmap_topic", costmap_topic); + node_.get_parameter("costmap_updates_topic", costmap_updates_topic); + node_.get_parameter("robot_base_frame", robot_base_frame_); + node_.get_parameter("transform_tolerance", transform_tolerance_); + + /* initialize costmap */ + costmap_sub_ = node_.create_subscription( + costmap_topic, 1000, + [this](const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { + costmap_received_ = true; + updateFullMap(msg); + }); + + // ros::topic::waitForMessage + RCLCPP_INFO(node_.get_logger(), + "Waiting for costmap to become available, topic: %s", + costmap_topic.c_str()); + while (!costmap_received_) { + rclcpp::spin_some(node_.get_node_base_interface()); + usleep(1000000); + } + // updateFullMap(costmap_msg); // this is already called in the callback of + // the costmap_sub_ + + /* subscribe to map updates */ + costmap_updates_sub_ = + node_.create_subscription( + costmap_updates_topic, 1000, + [this](const map_msgs::msg::OccupancyGridUpdate::SharedPtr msg) { + updatePartialMap(msg); + }); + + + /* tf transform is necessary for getRobotPose */ + auto last_error = node_.now(); + std::string tf_error; + while (rclcpp::ok() && + !tf_->canTransform(global_frame_, robot_base_frame_, + tf2::TimePointZero, tf2::durationFromSec(0.1), + &tf_error)) { + rclcpp::spin_some(node_.get_node_base_interface()); + if (last_error + tf2::durationFromSec(5.0) < node_.now()) { + RCLCPP_WARN(node_.get_logger(), + "Timed out waiting for transform from %s to %s to become " + "available " + "before subscribing to costmap, tf error: %s", + robot_base_frame_.c_str(), global_frame_.c_str(), + tf_error.c_str()); + last_error = node_.now(); + ; + } + // The error string will accumulate and errors will typically be the same, + // so the last + // will do for the warning above. Reset the string here to avoid + // accumulation. + tf_error.clear(); + } +} + +void Costmap2DClient::updateFullMap( + const nav_msgs::msg::OccupancyGrid::SharedPtr msg) +{ + global_frame_ = msg->header.frame_id; + + unsigned int size_in_cells_x = msg->info.width; + unsigned int size_in_cells_y = msg->info.height; + double resolution = msg->info.resolution; + double origin_x = msg->info.origin.position.x; + double origin_y = msg->info.origin.position.y; + + RCLCPP_DEBUG(node_.get_logger(), "received full new map, resizing to: %d, %d", + size_in_cells_x, size_in_cells_y); + costmap_.resizeMap(size_in_cells_x, size_in_cells_y, resolution, origin_x, + origin_y); + + // lock as we are accessing raw underlying map + auto* mutex = costmap_.getMutex(); + std::lock_guard lock(*mutex); + + // fill map with data + unsigned char* costmap_data = costmap_.getCharMap(); + size_t costmap_size = costmap_.getSizeInCellsX() * costmap_.getSizeInCellsY(); + RCLCPP_DEBUG(node_.get_logger(), "full map update, %lu values", costmap_size); + for (size_t i = 0; i < costmap_size && i < msg->data.size(); ++i) { + unsigned char cell_cost = static_cast(msg->data[i]); + costmap_data[i] = cost_translation_table__[cell_cost]; + } + RCLCPP_DEBUG(node_.get_logger(), "map updated, written %lu values", + costmap_size); +} + +void Costmap2DClient::updatePartialMap( + const map_msgs::msg::OccupancyGridUpdate::SharedPtr msg) +{ + RCLCPP_DEBUG(node_.get_logger(), "received partial map update"); + global_frame_ = msg->header.frame_id; + + if (msg->x < 0 || msg->y < 0) { + RCLCPP_DEBUG(node_.get_logger(), + "negative coordinates, invalid update. x: %d, y: %d", msg->x, + msg->y); + return; + } + + size_t x0 = static_cast(msg->x); + size_t y0 = static_cast(msg->y); + size_t xn = msg->width + x0; + size_t yn = msg->height + y0; + + // lock as we are accessing raw underlying map + auto* mutex = costmap_.getMutex(); + std::lock_guard lock(*mutex); + + size_t costmap_xn = costmap_.getSizeInCellsX(); + size_t costmap_yn = costmap_.getSizeInCellsY(); + + if (xn > costmap_xn || x0 > costmap_xn || yn > costmap_yn || + y0 > costmap_yn) { + RCLCPP_WARN(node_.get_logger(), + "received update doesn't fully fit into existing map, " + "only part will be copied. received: [%lu, %lu], [%lu, %lu] " + "map is: [0, %lu], [0, %lu]", + x0, xn, y0, yn, costmap_xn, costmap_yn); + } + + // update map with data + unsigned char* costmap_data = costmap_.getCharMap(); + size_t i = 0; + for (size_t y = y0; y < yn && y < costmap_yn; ++y) { + for (size_t x = x0; x < xn && x < costmap_xn; ++x) { + size_t idx = costmap_.getIndex(x, y); + unsigned char cell_cost = static_cast(msg->data[i]); + costmap_data[idx] = cost_translation_table__[cell_cost]; + ++i; + } + } +} + +geometry_msgs::msg::Pose Costmap2DClient::getRobotPose() const +{ + geometry_msgs::msg::PoseStamped robot_pose; + geometry_msgs::msg::Pose empty_pose; + robot_pose.header.frame_id = robot_base_frame_; + robot_pose.header.stamp = node_.now(); + + auto& clk = *node_.get_clock(); + + // get the global pose of the robot + try { + robot_pose = tf_->transform(robot_pose, global_frame_, + tf2::durationFromSec(transform_tolerance_)); + } catch (tf2::LookupException& ex) { + RCLCPP_ERROR_THROTTLE(node_.get_logger(), clk, 1000, + "No Transform available Error looking up robot pose: " + "%s\n", + ex.what()); + return empty_pose; + } catch (tf2::ConnectivityException& ex) { + RCLCPP_ERROR_THROTTLE(node_.get_logger(), clk, 1000, + "Connectivity Error looking up robot pose: %s\n", + ex.what()); + return empty_pose; + } catch (tf2::ExtrapolationException& ex) { + RCLCPP_ERROR_THROTTLE(node_.get_logger(), clk, 1000, + "Extrapolation Error looking up robot pose: %s\n", + ex.what()); + return empty_pose; + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR_THROTTLE(node_.get_logger(), clk, 1000, "Other error: %s\n", + ex.what()); + return empty_pose; + } + + return robot_pose.pose; +} + +std::array init_translation_table() +{ + std::array cost_translation_table; + + // lineary mapped from [0..100] to [0..255] + for (size_t i = 0; i < 256; ++i) { + cost_translation_table[i] = + static_cast(1 + (251 * (i - 1)) / 97); + } + + // special values: + cost_translation_table[0] = 0; // NO obstacle + cost_translation_table[99] = 253; // INSCRIBED obstacle + cost_translation_table[100] = 254; // LETHAL obstacle + cost_translation_table[static_cast(-1)] = 255; // UNKNOWN + + return cost_translation_table; +} + +} // namespace explore diff --git a/src/m-explore-ros2/explore/src/explore.cpp b/src/m-explore-ros2/explore/src/explore.cpp new file mode 100755 index 0000000..3df6f92 --- /dev/null +++ b/src/m-explore-ros2/explore/src/explore.cpp @@ -0,0 +1,477 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Robert Bosch LLC. + * Copyright (c) 2015-2016, Jiri Horner. + * Copyright (c) 2021, Carlos Alvarez, Juan Galvis. + * 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 +#include +#include + +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 +{ +Explore::Explore() + : Node("explore_node") + , logger_(this->get_logger()) + , tf_buffer_(this->get_clock()) + , tf_listener_(tf_buffer_) + , costmap_client_(*this, &tf_buffer_) + , prev_distance_(0) + , last_markers_count_(0) +{ + double timeout; + double min_frontier_size; + this->declare_parameter("planner_frequency", 1.0); + this->declare_parameter("progress_timeout", 30.0); + this->declare_parameter("visualize", false); + this->declare_parameter("potential_scale", 1e-3); + this->declare_parameter("orientation_scale", 0.0); + this->declare_parameter("gain_scale", 1.0); + this->declare_parameter("min_frontier_size", 0.5); + this->declare_parameter("return_to_init", false); + + this->get_parameter("planner_frequency", planner_frequency_); + this->get_parameter("progress_timeout", timeout); + this->get_parameter("visualize", visualize_); + this->get_parameter("potential_scale", potential_scale_); + this->get_parameter("orientation_scale", orientation_scale_); + this->get_parameter("gain_scale", gain_scale_); + 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; + move_base_client_ = + rclcpp_action::create_client( + this, ACTION_NAME); + + search_ = frontier_exploration::FrontierSearch(costmap_client_.getCostmap(), + potential_scale_, gain_scale_, + min_frontier_size, logger_); + + if (visualize_) { + marker_array_publisher_ = + this->create_publisher("explore/" + "frontier" + "s", + 10); + } + + // Subscription to resume or stop exploration + resume_subscription_ = this->create_subscription( + "explore/resume", 10, + std::bind(&Explore::resumeCallback, this, std::placeholders::_1)); + + RCLCPP_INFO(logger_, "Waiting to connect to move_base nav2 server"); + move_base_client_->wait_for_action_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; + } + } + + + initial_random_exploration_ = true; + random_goal_index_ = 0; + + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_real_distribution<> dis(-1.0, 1.0); + + for (int i = 0; i < 3; ++i) { + geometry_msgs::msg::Point p; + p.x = initial_pose_.position.x + dis(gen); + p.y = initial_pose_.position.y + dis(gen); + p.z = initial_pose_.position.z; + random_goals_.push_back(p); + } + + // Send first random goal + sendRandomGoal(); + RCLCPP_INFO(logger_, "Initial random exploration goals generated and first goal sent."); + + + + + exploring_timer_ = this->create_wall_timer( + std::chrono::milliseconds((uint16_t)(1000.0 / planner_frequency_)), + [this]() { makePlan(); }); + // Start exploration right away + makePlan(); +} + +void Explore::sendRandomGoal() { + if (random_goal_index_ >= random_goals_.size()) { + initial_random_exploration_ = false; + makePlan(); // resume normal frontier exploration + return; + } + + auto goal = nav2_msgs::action::NavigateToPose::Goal(); + goal.pose.pose.position = random_goals_[random_goal_index_]; + goal.pose.pose.orientation.w = 1.0; + goal.pose.header.frame_id = costmap_client_.getGlobalFrameID(); + goal.pose.header.stamp = this->now(); + + auto send_goal_options = + rclcpp_action::Client::SendGoalOptions(); + send_goal_options.result_callback = + [this](const NavigationGoalHandle::WrappedResult& result) { + random_goal_index_++; + sendRandomGoal(); + }; + + move_base_client_->async_send_goal(goal, send_goal_options); +} + + +Explore::~Explore() +{ + stop(); +} + +void Explore::resumeCallback(const std_msgs::msg::Bool::SharedPtr msg) +{ + if (msg->data) { + resume(); + } else { + stop(); + } +} + +void Explore::visualizeFrontiers( + const std::vector& frontiers) +{ + std_msgs::msg::ColorRGBA blue; + blue.r = 0; + blue.g = 0; + blue.b = 1.0; + blue.a = 1.0; + std_msgs::msg::ColorRGBA red; + red.r = 1.0; + red.g = 0; + red.b = 0; + red.a = 1.0; + std_msgs::msg::ColorRGBA green; + green.r = 0; + green.g = 1.0; + green.b = 0; + green.a = 1.0; + + RCLCPP_DEBUG(logger_, "visualising %lu frontiers", frontiers.size()); + visualization_msgs::msg::MarkerArray markers_msg; + std::vector& markers = markers_msg.markers; + visualization_msgs::msg::Marker m; + + m.header.frame_id = costmap_client_.getGlobalFrameID(); + m.header.stamp = this->now(); + m.ns = "frontiers"; + m.scale.x = 1.0; + m.scale.y = 1.0; + m.scale.z = 1.0; + m.color.r = 0; + m.color.g = 0; + m.color.b = 255; + m.color.a = 255; + // lives forever +#ifdef ELOQUENT + 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 + m.frame_locked = true; + + // weighted frontiers are always sorted + double min_cost = frontiers.empty() ? 0. : frontiers.front().cost; + + m.action = visualization_msgs::msg::Marker::ADD; + size_t id = 0; + for (auto& frontier : frontiers) { + m.type = visualization_msgs::msg::Marker::POINTS; + m.id = int(id); + // m.pose.position = {}; // compile warning + m.scale.x = 0.1; + m.scale.y = 0.1; + m.scale.z = 0.1; + m.points = frontier.points; + if (goalOnBlacklist(frontier.centroid)) { + m.color = red; + } else { + m.color = blue; + } + markers.push_back(m); + ++id; + m.type = visualization_msgs::msg::Marker::SPHERE; + m.id = int(id); + m.pose.position = frontier.initial; + // scale frontier according to its cost (costier frontiers will be smaller) + double scale = std::min(std::abs(min_cost * 0.4 / frontier.cost), 0.5); + m.scale.x = scale; + m.scale.y = scale; + m.scale.z = scale; + m.points = {}; + m.color = green; + markers.push_back(m); + ++id; + } + size_t current_markers_count = markers.size(); + + // delete previous markers, which are now unused + m.action = visualization_msgs::msg::Marker::DELETE; + for (; id < last_markers_count_; ++id) { + m.id = int(id); + markers.push_back(m); + } + + last_markers_count_ = current_markers_count; + marker_array_publisher_->publish(markers_msg); +} + +void Explore::makePlan() +{ + // find frontiers + auto pose = costmap_client_.getRobotPose(); + // get frontiers sorted according to cost + auto frontiers = search_.searchFrom(pose.position); + RCLCPP_DEBUG(logger_, "found %lu frontiers", frontiers.size()); + for (size_t i = 0; i < frontiers.size(); ++i) { + RCLCPP_DEBUG(logger_, "frontier %zd cost: %f", i, frontiers[i].cost); + } + + if (frontiers.empty()) { + RCLCPP_WARN(logger_, "No frontiers found, stopping."); + stop(true); + return; + } + + // publish frontiers as visualization markers + if (visualize_) { + visualizeFrontiers(frontiers); + } + + // find non blacklisted frontier + auto frontier = + std::find_if_not(frontiers.begin(), frontiers.end(), + [this](const frontier_exploration::Frontier& f) { + return goalOnBlacklist(f.centroid); + }); + if (frontier == frontiers.end()) { + RCLCPP_WARN(logger_, "All frontiers traversed/tried out, stopping."); + stop(true); + return; + } + geometry_msgs::msg::Point target_position = frontier->centroid; + + // time out if we are not making any progress + bool same_goal = same_point(prev_goal_, target_position); + + prev_goal_ = target_position; + if (!same_goal || prev_distance_ > frontier->min_distance) { + // we have different goal or we made some progress + last_progress_ = this->now(); + prev_distance_ = frontier->min_distance; + } + // black list if we've made no progress for a long time + if ((this->now() - last_progress_ > + tf2::durationFromSec(progress_timeout_)) && !resuming_) { + frontier_blacklist_.push_back(target_position); + RCLCPP_DEBUG(logger_, "Adding current goal to black list"); + makePlan(); + 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 + if (same_goal) { + return; + } + + RCLCPP_DEBUG(logger_, "Sending goal to move base nav2"); + + // send goal to move_base if we have something new to pursue + auto goal = nav2_msgs::action::NavigateToPose::Goal(); + goal.pose.pose.position = target_position; + goal.pose.pose.orientation.w = 1.; + goal.pose.header.frame_id = costmap_client_.getGlobalFrameID(); + goal.pose.header.stamp = this->now(); + + auto send_goal_options = + rclcpp_action::Client::SendGoalOptions(); + // std::bind(&Explore::feedback_callback, this, _1, _2); + send_goal_options.result_callback = + [this, + target_position](const NavigationGoalHandle::WrappedResult& result) { + reachedGoal(result, target_position); + }; + 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::SendGoalOptions(); + move_base_client_->async_send_goal(goal, send_goal_options); +} + +bool Explore::goalOnBlacklist(const geometry_msgs::msg::Point& goal) +{ + constexpr static size_t tolerace = 5; + nav2_costmap_2d::Costmap2D* costmap2d = costmap_client_.getCostmap(); + + // check if a goal is on the blacklist for goals that we're pursuing + for (auto& frontier_goal : frontier_blacklist_) { + double x_diff = fabs(goal.x - frontier_goal.x); + double y_diff = fabs(goal.y - frontier_goal.y); + + if (x_diff < tolerace * costmap2d->getResolution() && + y_diff < tolerace * costmap2d->getResolution()) + return true; + } + return false; +} + +void Explore::reachedGoal(const NavigationGoalHandle::WrappedResult& result, + const geometry_msgs::msg::Point& frontier_goal) +{ + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + RCLCPP_DEBUG(logger_, "Goal was successful"); + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_DEBUG(logger_, "Goal was aborted"); + frontier_blacklist_.push_back(frontier_goal); + 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; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_DEBUG(logger_, "Goal was canceled"); + // If goal canceled might be because exploration stopped from topic. Don't make new plan. + return; + default: + RCLCPP_WARN(logger_, "Unknown result code from move base nav2"); + break; + } + // find new goal immediately regardless of planning frequency. + // execute via timer to prevent dead lock in move_base_client (this is + // callback for sendGoal, which is called in makePlan). the timer must live + // until callback is executed. + // oneshot_ = relative_nh_.createTimer( + // ros::Duration(0, 0), [this](const ros::TimerEvent&) { makePlan(); }, + // true); + + // Because of the 1-thread-executor nature of ros2 I think timer is not + // needed. + makePlan(); +} + +void Explore::start() +{ + RCLCPP_INFO(logger_, "Exploration started."); +} + +void Explore::stop(bool finished_exploring) +{ + RCLCPP_INFO(logger_, "Exploration stopped."); + move_base_client_->async_cancel_all_goals(); + exploring_timer_->cancel(); + + 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 + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + // ROS1 code + /* + if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, + ros::console::levels::Debug)) { + ros::console::notifyLoggerLevelsChanged(); + } */ + rclcpp::spin( + std::make_shared()); // std::move(std::make_unique)? + rclcpp::shutdown(); + return 0; +} diff --git a/src/m-explore-ros2/explore/src/frontier_search.cpp b/src/m-explore-ros2/explore/src/frontier_search.cpp new file mode 100755 index 0000000..6085fd8 --- /dev/null +++ b/src/m-explore-ros2/explore/src/frontier_search.cpp @@ -0,0 +1,198 @@ +#include +#include + +#include +#include + +#include "nav2_costmap_2d/cost_values.hpp" + +namespace frontier_exploration +{ +using nav2_costmap_2d::FREE_SPACE; +using nav2_costmap_2d::LETHAL_OBSTACLE; +using nav2_costmap_2d::NO_INFORMATION; + +FrontierSearch::FrontierSearch(nav2_costmap_2d::Costmap2D* costmap, + double potential_scale, double gain_scale, + double min_frontier_size, rclcpp::Logger logger) + : costmap_(costmap) + , potential_scale_(potential_scale) + , gain_scale_(gain_scale) + , min_frontier_size_(min_frontier_size) + , logger_(logger) +{ +} + +std::vector +FrontierSearch::searchFrom(geometry_msgs::msg::Point position) +{ + std::vector frontier_list; + + // Sanity check that robot is inside costmap bounds before searching + unsigned int mx, my; + if (!costmap_->worldToMap(position.x, position.y, mx, my)) { + RCLCPP_ERROR(logger_, "[FrontierSearch] Robot out of costmap bounds, cannot search for frontiers"); + return frontier_list; + } + + // make sure map is consistent and locked for duration of search + std::lock_guard lock( + *(costmap_->getMutex())); + + map_ = costmap_->getCharMap(); + size_x_ = costmap_->getSizeInCellsX(); + size_y_ = costmap_->getSizeInCellsY(); + + // initialize flag arrays to keep track of visited and frontier cells + std::vector frontier_flag(size_x_ * size_y_, false); + std::vector visited_flag(size_x_ * size_y_, false); + + // initialize breadth first search + std::queue bfs; + + // find closest clear cell to start search + unsigned int clear, pos = costmap_->getIndex(mx, my); + if (nearestCell(clear, pos, FREE_SPACE, *costmap_)) { + bfs.push(clear); + } else { + bfs.push(pos); + RCLCPP_WARN(logger_, "[FrontierSearch] Could not find nearby clear cell to start search"); + } + visited_flag[bfs.front()] = true; + + while (!bfs.empty()) { + unsigned int idx = bfs.front(); + bfs.pop(); + + // iterate over 4-connected neighbourhood + for (unsigned nbr : nhood4(idx, *costmap_)) { + // add to queue all free, unvisited cells, use descending search in case + // initialized on non-free cell + if (map_[nbr] <= map_[idx] && !visited_flag[nbr]) { + visited_flag[nbr] = true; + bfs.push(nbr); + // check if cell is new frontier cell (unvisited, NO_INFORMATION, free + // neighbour) + } else if (isNewFrontierCell(nbr, frontier_flag)) { + frontier_flag[nbr] = true; + Frontier new_frontier = buildNewFrontier(nbr, pos, frontier_flag); + if (new_frontier.size * costmap_->getResolution() >= + min_frontier_size_) { + frontier_list.push_back(new_frontier); + } + } + } + } + + // set costs of frontiers + for (auto& frontier : frontier_list) { + frontier.cost = frontierCost(frontier); + } + std::sort( + frontier_list.begin(), frontier_list.end(), + [](const Frontier& f1, const Frontier& f2) { return f1.cost < f2.cost; }); + + return frontier_list; +} + +Frontier FrontierSearch::buildNewFrontier(unsigned int initial_cell, + unsigned int reference, + std::vector& frontier_flag) +{ + // initialize frontier structure + Frontier output; + output.centroid.x = 0; + output.centroid.y = 0; + output.size = 1; + output.min_distance = std::numeric_limits::infinity(); + + // record initial contact point for frontier + unsigned int ix, iy; + costmap_->indexToCells(initial_cell, ix, iy); + costmap_->mapToWorld(ix, iy, output.initial.x, output.initial.y); + + // push initial gridcell onto queue + std::queue bfs; + bfs.push(initial_cell); + + // cache reference position in world coords + unsigned int rx, ry; + double reference_x, reference_y; + costmap_->indexToCells(reference, rx, ry); + costmap_->mapToWorld(rx, ry, reference_x, reference_y); + + while (!bfs.empty()) { + unsigned int idx = bfs.front(); + bfs.pop(); + + // try adding cells in 8-connected neighborhood to frontier + for (unsigned int nbr : nhood8(idx, *costmap_)) { + // check if neighbour is a potential frontier cell + if (isNewFrontierCell(nbr, frontier_flag)) { + // mark cell as frontier + frontier_flag[nbr] = true; + unsigned int mx, my; + double wx, wy; + costmap_->indexToCells(nbr, mx, my); + costmap_->mapToWorld(mx, my, wx, wy); + + geometry_msgs::msg::Point point; + point.x = wx; + point.y = wy; + output.points.push_back(point); + + // update frontier size + output.size++; + + // update centroid of frontier + output.centroid.x += wx; + output.centroid.y += wy; + + // determine frontier's distance from robot, going by closest gridcell + // to robot + double distance = sqrt(pow((double(reference_x) - double(wx)), 2.0) + + pow((double(reference_y) - double(wy)), 2.0)); + if (distance < output.min_distance) { + output.min_distance = distance; + output.middle.x = wx; + output.middle.y = wy; + } + + // add to queue for breadth first search + bfs.push(nbr); + } + } + } + + // average out frontier centroid + output.centroid.x /= output.size; + output.centroid.y /= output.size; + return output; +} + +bool FrontierSearch::isNewFrontierCell(unsigned int idx, + const std::vector& frontier_flag) +{ + // check that cell is unknown and not already marked as frontier + if (map_[idx] != NO_INFORMATION || frontier_flag[idx]) { + return false; + } + + // frontier cells should have at least one cell in 4-connected neighbourhood + // that is free + for (unsigned int nbr : nhood4(idx, *costmap_)) { + if (map_[nbr] == FREE_SPACE) { + return true; + } + } + + return false; +} + +double FrontierSearch::frontierCost(const Frontier& frontier) +{ + return (potential_scale_ * frontier.min_distance * + costmap_->getResolution()) - + (gain_scale_ * frontier.size * costmap_->getResolution()); +} +} // namespace frontier_exploration diff --git a/src/m-explore-ros2/explore/test/test_explore.cpp b/src/m-explore-ros2/explore/test/test_explore.cpp new file mode 100755 index 0000000..2fe0913 --- /dev/null +++ b/src/m-explore-ros2/explore/test/test_explore.cpp @@ -0,0 +1,77 @@ +/********************************************************************* + * + * 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 +#include +#include + +#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(); +} diff --git a/src/turtlebot3_simulations/turtlebot3_gazebo/CHANGELOG.rst b/src/turtlebot3_simulations/turtlebot3_gazebo/CHANGELOG.rst new file mode 100755 index 0000000..09601f1 --- /dev/null +++ b/src/turtlebot3_simulations/turtlebot3_gazebo/CHANGELOG.rst @@ -0,0 +1,187 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package turtlebot3_gazebo +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.3.8 (2025-07-10) +------------------ +* None + +2.3.6 (2025-06-19) +------------------ +* None + +2.3.4 (2025-05-28) +------------------ +* None + +2.3.0 (2025-02-17) +------------------ +* Added multi-robot launch functionality +* Updated robot mesh in Gazebo and RViz +* Added launch file for TurtleBot3 Autorace 2020 +* Added plugins to the models of Autorace 2020 +* Contributors: Hyungyu Kim + +2.2.6 (2022-05-26) +------------------ +* ROS 2 Humble Hawksbill supported +* Contributors: Will Son + +2.2.5 (2021-08-25) +------------------ +* Release for ROS 2 Rolling +* Contributors: Will Son + +2.2.4 (2021-06-14) +------------------ +* Release for ROS 2 Galactic +* Separate world and robot models(#162) +* Clean up unncessary files +* Use turtlebot3_common mesh modeling +* Independent turtlebot3_simulations package +* Contributors: Joep Tool, Will Son + +2.2.3 (2021-04-12) +------------------ +* Update required keyword arguments +* Clear up exec_depend +* Fix Waffle Pi wheel inertia +* Contributors: ruffsl, Will Son + +2.2.2 (2021-02-24) +------------------ +* Remove shared objects built in older version +* Contributors: Will Son + +2.2.1 (2021-01-13) +------------------ +* Eloquent Elusor EOL +* Add missing imu joint in sdf +* Append Gazebo model path +* Portable fix, launch description revise +* Ament lint applied +* Contributors: minwoominwoominwoo7, Rayman, seanyen, ashe kim, Will Son + +2.2.0 (2020-06-29) +------------------ +* TurtleBot3 Drive node implementation +* Additional Gazebo maps added +* argument tags in the sdf file replaced with remapping tags +* Low polygon 3D modeling applied for simulation +* Contributors: Ryan Shim, Mikael Arguedas, Will Son + +2.1.0 (2019-09-10) +------------------ +* Added turtlebot3_house and related world, model files +* Contributors: Ryan Shim + +2.0.1 (2019-09-05) +------------------ +* Modified dependency packages +* Modified launch directory +* Added a launch file for robot state publisher +* Contributors: Darby Lim, Pyo + +2.0.0 (2019-08-20) +------------------ +* Supported ROS 2 Dashing Diademata +* Updated the CHANGELOG and version to release binary packages +* Contributors: Darby Lim, Pyo + +1.3.0 (2020-06-29) +------------------ +* Turtlebot3 Autorace 2020 implemented +* Remove the plugin_path from gazebo_ros export +* Remove *nix path separator +* Contributors: Ashe Kim, Ben Wolsieffer, Sean Yen + +1.2.0 (2019-01-22) +------------------ +* moved into `#65 `_ +* modified ML stage +* delete unused param +* update algorithm and modified variable more clearly +* Contributors: Darby Lim, Gilbert, Louise Poubel, Pyo + +1.1.0 (2018-07-20) +------------------ +* modified uri path +* modified autorace +* delete remap +* Contributors: Darby Lim, Gilbert, Pyo + +1.0.2 (2018-06-01) +------------------ +* added mission.launch modified model.sdf +* deleted turtlebot3's gazebo plugins +* modified autorace gazebo +* merged pull request `#53 `_ `#52 `_ `#51 `_ `#50 `_ `#49 `_ +* Contributors: Gilbert, Darby Lim, Pyo + +1.0.1 (2018-05-30) +------------------ +* resolving dependency issues: + http://build.ros.org/job/Kbin_dj_dJ64__turtlebot3_gazebo__debian_jessie_amd64__binary/2/ +* Contributors: Pyo + +1.0.0 (2018-05-29) +------------------ +* added world for turtlebot3_autorace +* added world for turtlebot3_machine_learning +* merged pull request `#46 `_ from AuTURBO/develop + add turtlebot3_autorace world' +* merged pull request `#48 `_ `#47 `_ `#44 `_ `#42 `_ `#41 `_ +* Contributors: Darby Lim, Gilbert, hyunoklee, Pyo + +0.2.4 (2018-03-14) +------------------ +* None + +0.2.3 (2018-03-14) +------------------ +* solved DuplicateVersionsException error +* Contributors: Pyo + +0.2.2 (2018-03-14) +------------------ +* None + +0.2.1 (2018-03-14) +------------------ +* added worlds for gazebo and turtlebot3 +* Contributors: Darby Lim + +0.2.0 (2018-03-13) +------------------ +* added slam with multiple tb3 +* added multi example +* added turtlebot3_house +* modified cmake file +* modified spwn model name +* modified multi slam param +* modified camera position +* modified folder name +* Contributors: Darby Lim + +0.1.7 (2017-08-16) +------------------ +* renamed missed the install rule (worlds -> models) +* Contributors: Darby Lim, Tully Foote + +0.1.6 (2017-08-14) +------------------ +* modified folder name and model path +* updated rviz and add static tf publisher for depth camera +* Contributors: Darby Lim + +0.1.5 (2017-06-09) +------------------ +* modified make files for dependencies +* updated turtlebot3 sim +* updated world config +* Contributors: Darby Lim + +0.1.4 (2017-05-23) +------------------ +* added as new meta-packages and version update (0.1.4) +* Contributors: Darby Lim, Pyo diff --git a/src/turtlebot3_simulations/turtlebot3_gazebo/CMakeLists.txt b/src/turtlebot3_simulations/turtlebot3_gazebo/CMakeLists.txt new file mode 100755 index 0000000..2a1f27b --- /dev/null +++ b/src/turtlebot3_simulations/turtlebot3_gazebo/CMakeLists.txt @@ -0,0 +1,139 @@ +################################################################################ +# Set minimum required version of cmake, project name and compile options +################################################################################ +cmake_minimum_required(VERSION 3.5) +project(turtlebot3_gazebo) + +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() + +if(MSVC) + add_compile_definitions(_USE_MATH_DEFINES) +endif() + +################################################################################ +# Find ament packages and libraries for ament and system dependencies +# NOTE: Migrating from Gazebo Classic to Gazebo Sim (gz-8 / Harmonic) +################################################################################ +find_package(ament_cmake REQUIRED) + +# ROS 2 core packages +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(tf2 REQUIRED) + +# Gazebo Sim (Harmonic) libraries +find_package(gz-sim8 REQUIRED) +find_package(gz-msgs10 REQUIRED) +find_package(gz-transport13 REQUIRED) +find_package(gz-physics7 REQUIRED) +find_package(gz-math7 REQUIRED) +find_package(gz-common5 REQUIRED) +find_package(gz-plugin2 REQUIRED) +find_package(sdformat14 REQUIRED) + +# Optional: ros_gz bridge packages (for launch integration) +find_package(ros_gz_bridge QUIET) +find_package(ros_gz_sim QUIET) + +################################################################################ +# Build +################################################################################ + +include_directories( + include + ${gz-sim8_INCLUDE_DIRS} + ${gz-msgs10_INCLUDE_DIRS} + ${gz-transport13_INCLUDE_DIRS} + ${gz-physics7_INCLUDE_DIRS} + ${gz-math7_INCLUDE_DIRS} + ${gz-common5_INCLUDE_DIRS} + ${gz-plugin2_INCLUDE_DIRS} + ${sdformat14_INCLUDE_DIRS} +) + +set(dependencies + "geometry_msgs" + "nav_msgs" + "rclcpp" + "sensor_msgs" + "tf2" +) + +# --- Executable: turtlebot3_drive (ROS 2 navigation node, no gazebo deps) --- +set(EXEC_NAME "turtlebot3_drive") +add_executable(${EXEC_NAME} src/turtlebot3_drive.cpp) +ament_target_dependencies(${EXEC_NAME} ${dependencies}) + +# --- Shared Libraries: Custom Gazebo Sim System Plugins --- +# NOTE: These plugins originally used the Gazebo Classic API (gazebo::ModelPlugin) +# with headers like . They must be rewritten to use the +# gz-sim System API (ISystemPreUpdate, EntityComponentManager, etc.) +# before they can compile. Default OFF until porting is done. +option(BUILD_GAZEBO_PLUGINS "Build custom Gazebo Sim system plugins" OFF) + +if(BUILD_GAZEBO_PLUGINS) + set(GZ_PLUGIN_LIBRARIES + gz-sim8::gz-sim8 + gz-msgs10::gz-msgs10 + gz-transport13::gz-transport13 + gz-physics7::gz-physics7 + gz-math7::gz-math7 + gz-common5::gz-common5 + gz-plugin2::gz-plugin2 + sdformat14::sdformat14 + ) + + add_library(traffic_light_plugin SHARED src/traffic_light_plugin.cpp) + target_link_libraries(traffic_light_plugin ${GZ_PLUGIN_LIBRARIES}) + + add_library(traffic_bar_plugin SHARED src/traffic_bar_plugin.cpp) + target_link_libraries(traffic_bar_plugin ${GZ_PLUGIN_LIBRARIES}) + + add_library(obstacle1 SHARED src/obstacle1.cpp) + target_link_libraries(obstacle1 ${GZ_PLUGIN_LIBRARIES}) + + add_library(obstacle2 SHARED src/obstacle2.cpp) + target_link_libraries(obstacle2 ${GZ_PLUGIN_LIBRARIES}) + + add_library(obstacles SHARED src/obstacles.cpp) + target_link_libraries(obstacles ${GZ_PLUGIN_LIBRARIES}) +endif() + +################################################################################ +# Install +################################################################################ +install(TARGETS ${EXEC_NAME} + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY launch config models rviz urdf worlds scripts gui + DESTINATION share/${PROJECT_NAME}/ +) + +# Install Python scripts with correct permissions +install(PROGRAMS scripts/scan_frame_fix.py scripts/camera_topic_remap.py + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +################################################################################ +# Export & Macro for ament package +################################################################################ +ament_export_include_directories(include) +ament_export_dependencies(geometry_msgs) +ament_export_dependencies(nav_msgs) +ament_export_dependencies(rclcpp) +ament_export_dependencies(sensor_msgs) +ament_export_dependencies(tf2) +ament_package() diff --git a/src/turtlebot3_simulations/turtlebot3_gazebo/config/turtlebot3_gz_bridge.yaml b/src/turtlebot3_simulations/turtlebot3_gazebo/config/turtlebot3_gz_bridge.yaml new file mode 100644 index 0000000..bb49409 --- /dev/null +++ b/src/turtlebot3_simulations/turtlebot3_gazebo/config/turtlebot3_gz_bridge.yaml @@ -0,0 +1,42 @@ +# TurtleBot3 Burger - Gz Sim to ROS 2 Bridge Configuration +# For use with ros_gz_bridge bridge_node +# +# Key migration notes (Gazebo Classic -> Gazebo Sim): +# 1. gz-sim-diff-drive publishes Odometry but NOT TF (unlike Classic). +# TF is bridged from /world//pose/info (gz.msgs.Pose_V -> TFMessage). +# 2. Sensor frame_ids use Gazebo scoped names (e.g. burger/base_scan/...). +# Overridden below via 'frame_id' field to match URDF/robot_state_publisher tree. + +# === TF: bridge Gazebo model tf to ROS2 /tf === +# /model/burger/tf publishes gz.msgs.Pose_V (NOT Pose!) +# ros_gz_bridge converts Pose_V -> TFMessage correctly. +- ros_topic_name: /tf + ros_type_name: tf2_msgs/msg/TFMessage + gz_topic_name: /model/burger/tf + gz_type_name: gz.msgs.Pose_V + direction: GZ_TO_ROS + +# === Sensor bridges === +- ros_topic_name: /scan_raw + ros_type_name: sensor_msgs/msg/LaserScan + gz_topic_name: /world/default/model/burger/link/base_scan/sensor/hls_lfcd_lds/scan + gz_type_name: gz.msgs.LaserScan + direction: GZ_TO_ROS + +- ros_topic_name: /clock + ros_type_name: rosgraph_msgs/msg/Clock + gz_topic_name: /clock + gz_type_name: gz.msgs.Clock + direction: GZ_TO_ROS + +- ros_topic_name: /cmd_vel + ros_type_name: geometry_msgs/msg/Twist + gz_topic_name: /model/burger/cmd_vel + gz_type_name: gz.msgs.Twist + direction: ROS_TO_GZ + +- ros_topic_name: /odom + ros_type_name: nav_msgs/msg/Odometry + gz_topic_name: /model/burger/odometry + gz_type_name: gz.msgs.Odometry + direction: GZ_TO_ROS diff --git a/src/turtlebot3_simulations/turtlebot3_gazebo/gui/office_gui.config b/src/turtlebot3_simulations/turtlebot3_gazebo/gui/office_gui.config new file mode 100644 index 0000000..629f569 --- /dev/null +++ b/src/turtlebot3_simulations/turtlebot3_gazebo/gui/office_gui.config @@ -0,0 +1,270 @@ + + + + + + + + 1400 + 900 +