init

README.md

@@ -0,0 +1,175 @@
+# office_gzweb
+
+## 项目说明
+
+本项目用于 Ubuntu + ROS 2 Humble 下的 TurtleBot3 办公室场景自动探索与建图，包含：
+
+- Gazebo Sim 8 (Harmonic) 仿真
+- Nav2 导航栈
+- SLAM Toolbox 在线建图
+- explore_lite 自动探索
+- gzweb 网页端 3D 可视化
+- 自动保存地图并输出路径
+
+核心目标：一条命令启动，探索结束后自动保存地图并退出。
+
+---
+
+## 环境要求
+
+- Ubuntu 22.04
+- ROS 2 Humble
+- Gazebo Sim Harmonic
+- Node.js / npm（用于 `gzweb-demo`）
+
+---
+
+## 关键目录与文件
+
+- 仿真与模型：`turtlebot3_simulations/turtlebot3_gazebo/`
+- 探索节点：`m-explore-ros2/explore/`
+- 一键启动脚本：`run_all.sh`
+- 一键停止脚本：`run_all_stop.sh`
+- A+B 启动脚本：`run_ab.sh`
+- WebSocket 配置：`websocket.sdf`
+- gzweb 前端：`gzweb-demo/gzweb-demo/`
+- 地图输出目录：`turtlebot3_simulations/turtlebot3_gazebo/map/`
+
+---
+
+## 一键运行（推荐）
+
+### 1) 启动自动流程
+
+```bash
+cd /home/xh2204/office_gzweb/vlm_office-main
+./run_all.sh mapping --explore --gzweb
+```
+
+执行内容：
+
+1. 启动 Gazebo Sim + ROS bridge + SLAM + RViz  
+2. 启动 gzweb websocket 与前端  
+3. 启动 Nav2  
+4. 启动 explore_lite 自动探索  
+5. 自动检测探索结束（带超时）  
+6. 自动保存地图并打印路径  
+7. 自动停止全部进程并退出
+
+### 2) 停止全部进程
+
+```bash
+./run_all_stop.sh
+```
+
+---
+
+## 端口说明
+
+- `5173`：gzweb 前端（Vite）
+- `9002`：Gazebo WebSocket
+
+浏览器访问：
+
+- [http://localhost:5173](http://localhost:5173)
+
+---
+
+## 地图保存结果
+
+默认保存到：
+
+- `turtlebot3_simulations/turtlebot3_gazebo/map/office_map.yaml`
+- `turtlebot3_simulations/turtlebot3_gazebo/map/office_map.pgm`
+
+`run_all.sh` 保存完成后会在终端打印绝对路径。
+
+---
+
+## 常用脚本参数
+
+### `run_all.sh`
+
+```bash
+./run_all.sh mapping [--explore] [--gzweb] [--no-rviz] [--world PATH]
+./run_all.sh localization [--gzweb] [--no-rviz] [--map PATH] [--world PATH]
+```
+
+可配置环境变量：
+
+- `ROBOT_MODEL`（默认 `waffle`）
+- `GZ_PARTITION`（默认 `tb3_office_gz`）
+- `SLEEP_NAV2`（默认 `18` 秒）
+- `EXPLORE_WAIT_TIMEOUT`（默认 `420` 秒）
+
+---
+
+## 关键实现（重要代码点）
+
+### 1) 仿真/导航/探索总控
+
+- `run_all.sh`
+  - 统一启动 A(sim)、B(nav2)、C(explore)、D(websocket)、E(gzweb 前端)
+  - 自动端口检查与冲突清理（`5173/9002`）
+  - 探索阶段终端计时输出（explore elapsed）
+  - 探索结束自动保存地图并退出
+
+### 2) Gazebo + ROS 集成启动
+
+- `turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_office_gz.launch.py`
+  - office world 启动
+  - `/clock`、`/scan`、`/odom`、`/tf` 等桥接
+  - SLAM 与 RViz 延迟启动控制
+  - `GZ_PARTITION` 隔离避免串到其他 Gazebo 会话
+
+### 3) gzweb 纹理与材质修复
+
+- `gzweb/gzweb/include/ColladaLoader.js`
+  - 修复 `RGBFormat is not defined` 问题
+  - 修复 `model://` 纹理 URI 拼接错误（绝对 URI 不再错误拼接）
+  - 重新构建输出到 `gzweb/gzweb/dist/`
+
+### 4) 模型颜色兜底
+
+- `office_desk/model.sdf`
+- `office_chair/model.sdf`
+- `office_couch/model.sdf`
+
+为关键家具增加 fallback 材质参数，避免网页端纹理异常时完全黑白。
+
+---
+
+## 典型问题与处理
+
+- gzweb 无法连接：检查 `9002` 是否被占用，`run_all.sh` 会自动清理并重启
+- 前端打不开：检查 `5173`，并查看 `logs/run_all_*/E_gzweb_frontend.log`
+- 纹理丢失/颜色异常：检查 `ColladaLoader.js` 与 `GZ_SIM_RESOURCE_PATH`
+- 探索长期不结束：`EXPLORE_WAIT_TIMEOUT` 超时后自动强制收尾并保存地图
+
+---
+
+## 日志位置
+
+每次运行会生成：
+
+- `logs/run_all_YYYYMMDD_HHMMSS/`
+  - `A_sim.log`
+  - `B_nav2.log`
+  - `C_explore.log`
+  - `D_websocket.log`
+  - `E_gzweb_frontend.log`
+
+---
+
+## 快速命令参考
+
+```bash
+# 启动（推荐）
+./run_all.sh mapping --explore --gzweb
+
+# 强制停止全部
+./run_all_stop.sh
+
+# 仅 A+B（不含 explore/gzweb）
+./run_ab.sh mapping
+```

docker/Dockerfile

@@ -0,0 +1,40 @@
+FROM docker.1ms.run/library/ros:humble-ros-base
+
+SHELL ["/bin/bash", "-c"]
+
+ENV DEBIAN_FRONTEND=noninteractive
+
+RUN apt-get update && apt-get install -y --no-install-recommends \
+    python3-colcon-common-extensions \
+    ros-humble-navigation2 \
+    ros-humble-nav2-bringup \
+    ros-humble-slam-toolbox \
+    ros-humble-rviz2 \
+    ros-humble-turtlebot3 \
+    ros-humble-turtlebot3-msgs \
+    ros-humble-ros-gzharmonic \
+    ros-humble-robot-state-publisher \
+    gz-sim8-cli \
+    gz-launch7-cli \
+    && rm -rf /var/lib/apt/lists/*
+
+WORKDIR /workspaces/vlm_office
+RUN mkdir -p src
+
+COPY turtlebot3_simulations src/turtlebot3_simulations
+COPY m-explore-ros2 src/m-explore-ros2
+COPY docker/entrypoint.sh /entrypoint.sh
+COPY docker/run_task1.sh /run_task1.sh
+COPY docker/save_map.sh /save_map.sh
+
+RUN chmod +x /entrypoint.sh /run_task1.sh /save_map.sh && \
+    source /opt/ros/humble/setup.bash && \
+    colcon build --symlink-install \
+      --packages-select turtlebot3_gazebo explore_lite
+
+ENV TURTLEBOT3_MODEL=burger
+ENV NAV2_MAP_BASENAME=/data/maps/office_map
+ENV GZ_SIM_RESOURCE_PATH=/workspaces/vlm_office/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_office:/workspaces/vlm_office/src/turtlebot3_simulations/turtlebot3_gazebo/models
+
+ENTRYPOINT ["/entrypoint.sh"]
+CMD ["bash"]

docker/entrypoint.sh

@@ -0,0 +1,13 @@
+#!/bin/bash
+set -e
+
+source /opt/ros/humble/setup.bash
+
+if [ -f /workspaces/vlm_office/install/setup.bash ]; then
+  source /workspaces/vlm_office/install/setup.bash
+fi
+
+export TURTLEBOT3_MODEL="${TURTLEBOT3_MODEL:-burger}"
+export GZ_SIM_RESOURCE_PATH="/workspaces/vlm_office/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_office:/workspaces/vlm_office/src/turtlebot3_simulations/turtlebot3_gazebo/models${GZ_SIM_RESOURCE_PATH:+:$GZ_SIM_RESOURCE_PATH}"
+
+exec "$@"

docker/run_task1.sh

@@ -0,0 +1,52 @@
+#!/bin/bash
+set -eo pipefail
+
+source /opt/ros/humble/setup.bash
+source /workspaces/vlm_office/install/setup.bash
+
+export TURTLEBOT3_MODEL="${TURTLEBOT3_MODEL:-burger}"
+export GZ_SIM_RESOURCE_PATH="/workspaces/vlm_office/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_office:/workspaces/vlm_office/src/turtlebot3_simulations/turtlebot3_gazebo/models${GZ_SIM_RESOURCE_PATH:+:$GZ_SIM_RESOURCE_PATH}"
+
+HEADLESS="${HEADLESS:-false}"
+ENABLE_RVIZ="${ENABLE_RVIZ:-true}"
+
+PIDS=()
+
+cleanup() {
+  for pid in "${PIDS[@]:-}"; do
+    kill "$pid" 2>/dev/null || true
+  done
+  wait || true
+}
+
+trap cleanup EXIT INT TERM
+
+# Gazebo Sim 8 + TurtleBot3 + ros_gz_bridge (same ROS topics as Classic: /scan /odom /cmd_vel /tf /clock)
+ros2 launch turtlebot3_gazebo turtlebot3_office_gz.launch.py \
+  robot_model:="$TURTLEBOT3_MODEL" \
+  x_pose:=0.0 y_pose:=0.0 use_sim_time:=true headless:="$HEADLESS" \
+  show_rviz:=false start_slam:=false &
+PIDS+=($!)
+
+sleep 14
+
+ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True &
+PIDS+=($!)
+
+sleep 10
+
+ros2 launch slam_toolbox online_async_launch.py use_sim_time:=True &
+PIDS+=($!)
+
+sleep 10
+
+if [ "$ENABLE_RVIZ" = "true" ]; then
+  ros2 run rviz2 rviz2 -d /opt/ros/humble/share/nav2_bringup/rviz/nav2_default_view.rviz &
+  PIDS+=($!)
+  sleep 3
+fi
+
+ros2 launch explore_lite explore.launch.py use_sim_time:=true &
+PIDS+=($!)
+
+wait

docker/save_map.sh

@@ -0,0 +1,10 @@
+#!/bin/bash
+set -eo pipefail
+
+source /opt/ros/humble/setup.bash
+source /workspaces/vlm_office/install/setup.bash
+
+MAP_BASENAME="${NAV2_MAP_BASENAME:-/data/maps/office_map}"
+mkdir -p "$(dirname "$MAP_BASENAME")"
+
+ros2 run nav2_map_server map_saver_cli -f "$MAP_BASENAME"

m-explore-ros2/.gitignore

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

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.

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
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+
+
+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()

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

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

m-explore-ros2/explore/doc/wiki_doc.txt

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

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

m-explore-ros2/explore/include/explore/costmap_tools.h

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

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 <explore/costmap_client.h>
+#include <explore/frontier_search.h>
+#include <geometry_msgs/msg/pose_stamped.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <chrono>
+#include <cmath>
+#include <geometry_msgs/msg/point.hpp>
+#include <memory>
+#include <mutex>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/bool.hpp>
+#include <std_msgs/msg/color_rgba.hpp>
+#include <string>
+#include <vector>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#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<nav2_msgs::action::NavigateToPose>;
+
+private:
+  /**
+   * @brief  Make a global plan
+   */
+   
+  bool initial_random_exploration_;
+  size_t random_goal_index_;
+  std::vector<geometry_msgs::msg::Point> random_goals_;
+
+
+  void makePlan();
+
+  // /**
+  //  * @brief  Publish a frontiers as markers
+  //  */
+  void visualizeFrontiers(
+      const std::vector<frontier_exploration::Frontier>& frontiers);
+
+  bool goalOnBlacklist(const geometry_msgs::msg::Point& goal);
+
+  NavigationGoalHandle::SharedPtr navigation_goal_handle_;
+  // void
+  // goal_response_callback(std::shared_future<NavigationGoalHandle::SharedPtr>
+  // future);
+  void reachedGoal(const NavigationGoalHandle::WrappedResult& result,
+                   const geometry_msgs::msg::Point& frontier_goal);
+
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
+      marker_array_publisher_;
+  rclcpp::Logger logger_;
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+
+  Costmap2DClient costmap_client_;
+  rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SharedPtr
+      move_base_client_;
+  frontier_exploration::FrontierSearch search_;
+  rclcpp::TimerBase::SharedPtr exploring_timer_;
+  // 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_;
+  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

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<geometry_msgs::msg::Point> 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<Frontier> searchFrom(geometry_msgs::msg::Point position);
+
+protected:
+  /**
+   * @brief Starting from an initial cell, build a frontier from valid adjacent
+   * cells
+   * @param initial_cell Index of cell to start frontier building
+   * @param reference Reference index to calculate position from
+   * @param frontier_flag Flag vector indicating which cells are already marked
+   * as frontiers
+   * @return new frontier
+   */
+  Frontier buildNewFrontier(unsigned int initial_cell, unsigned int reference,
+                            std::vector<bool>& frontier_flag);
+
+  /**
+   * @brief isNewFrontierCell Evaluate if candidate cell is a valid candidate
+   * for a new frontier.
+   * @param idx Index of candidate cell
+   * @param frontier_flag Flag vector indicating which cells are already marked
+   * as frontiers
+   * @return true if the cell is frontier cell
+   */
+  bool isNewFrontierCell(unsigned int idx,
+                         const std::vector<bool>& frontier_flag);
+
+  /**
+   * @brief computes frontier cost
+   * @details cost function is defined by potential_scale and gain_scale
+   *
+   * @param frontier frontier for which compute the cost
+   * @return cost of the frontier
+   */
+  double frontierCost(const Frontier& frontier);
+
+private:
+  nav2_costmap_2d::Costmap2D* costmap_;
+  unsigned char* map_;
+  unsigned int size_x_, size_y_;
+  double potential_scale_, gain_scale_;
+  double min_frontier_size_;
+  rclcpp::Logger logger_;
+};
+}  // namespace frontier_exploration
+#endif

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

m-explore-ros2/explore/package.xml

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

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 <explore/costmap_client.h>
+#include <unistd.h>
+
+#include <functional>
+#include <mutex>
+#include <string>
+
+namespace explore
+{
+std::array<unsigned char, 256> init_translation_table();
+static const std::array<unsigned char, 256> cost_translation_table__ =
+    init_translation_table();
+
+Costmap2DClient::Costmap2DClient(rclcpp::Node& node, const tf2_ros::Buffer* tf)
+  : tf_(tf), node_(node)
+{
+  std::string costmap_topic;
+  std::string costmap_updates_topic;
+
+  node_.declare_parameter<std::string>("costmap_topic", std::string("costmap"));
+  node_.declare_parameter<std::string>("costmap_updates_topic",
+                                       std::string("costmap_updates"));
+  node_.declare_parameter<std::string>("robot_base_frame", std::string("base_"
+                                                                       "link"));
+  // transform tolerance is used for all tf transforms here
+  node_.declare_parameter<double>("transform_tolerance", 0.3);
+
+  node_.get_parameter("costmap_topic", costmap_topic);
+  node_.get_parameter("costmap_updates_topic", costmap_updates_topic);
+  node_.get_parameter("robot_base_frame", robot_base_frame_);
+  node_.get_parameter("transform_tolerance", transform_tolerance_);
+
+  /* initialize costmap */
+  costmap_sub_ = node_.create_subscription<nav_msgs::msg::OccupancyGrid>(
+      costmap_topic, 1000,
+      [this](const nav_msgs::msg::OccupancyGrid::SharedPtr msg) {
+        costmap_received_ = true;
+        updateFullMap(msg);
+      });
+
+  // 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<map_msgs::msg::OccupancyGridUpdate>(
+          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<nav2_costmap_2d::Costmap2D::mutex_t> lock(*mutex);
+
+  // fill map with data
+  unsigned char* costmap_data = costmap_.getCharMap();
+  size_t costmap_size = costmap_.getSizeInCellsX() * costmap_.getSizeInCellsY();
+  RCLCPP_DEBUG(node_.get_logger(), "full map update, %lu values", costmap_size);
+  for (size_t i = 0; i < costmap_size && i < msg->data.size(); ++i) {
+    unsigned char cell_cost = static_cast<unsigned char>(msg->data[i]);
+    costmap_data[i] = cost_translation_table__[cell_cost];
+  }
+  RCLCPP_DEBUG(node_.get_logger(), "map updated, written %lu values",
+               costmap_size);
+}
+
+void Costmap2DClient::updatePartialMap(
+    const map_msgs::msg::OccupancyGridUpdate::SharedPtr msg)
+{
+  RCLCPP_DEBUG(node_.get_logger(), "received partial map update");
+  global_frame_ = msg->header.frame_id;
+
+  if (msg->x < 0 || msg->y < 0) {
+    RCLCPP_DEBUG(node_.get_logger(),
+                 "negative coordinates, invalid update. x: %d, y: %d", msg->x,
+                 msg->y);
+    return;
+  }
+
+  size_t x0 = static_cast<size_t>(msg->x);
+  size_t y0 = static_cast<size_t>(msg->y);
+  size_t xn = msg->width + x0;
+  size_t yn = msg->height + y0;
+
+  // lock as we are accessing raw underlying map
+  auto* mutex = costmap_.getMutex();
+  std::lock_guard<nav2_costmap_2d::Costmap2D::mutex_t> lock(*mutex);
+
+  size_t costmap_xn = costmap_.getSizeInCellsX();
+  size_t costmap_yn = costmap_.getSizeInCellsY();
+
+  if (xn > costmap_xn || x0 > costmap_xn || yn > costmap_yn ||
+      y0 > costmap_yn) {
+    RCLCPP_WARN(node_.get_logger(),
+                "received update doesn't fully fit into existing map, "
+                "only part will be copied. received: [%lu, %lu], [%lu, %lu] "
+                "map is: [0, %lu], [0, %lu]",
+                x0, xn, y0, yn, costmap_xn, costmap_yn);
+  }
+
+  // update map with data
+  unsigned char* costmap_data = costmap_.getCharMap();
+  size_t i = 0;
+  for (size_t y = y0; y < yn && y < costmap_yn; ++y) {
+    for (size_t x = x0; x < xn && x < costmap_xn; ++x) {
+      size_t idx = costmap_.getIndex(x, y);
+      unsigned char cell_cost = static_cast<unsigned char>(msg->data[i]);
+      costmap_data[idx] = cost_translation_table__[cell_cost];
+      ++i;
+    }
+  }
+}
+
+geometry_msgs::msg::Pose Costmap2DClient::getRobotPose() const
+{
+  geometry_msgs::msg::PoseStamped robot_pose;
+  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<unsigned char, 256> init_translation_table()
+{
+  std::array<unsigned char, 256> cost_translation_table;
+
+  // lineary mapped from [0..100] to [0..255]
+  for (size_t i = 0; i < 256; ++i) {
+    cost_translation_table[i] =
+        static_cast<unsigned char>(1 + (251 * (i - 1)) / 97);
+  }
+
+  // special values:
+  cost_translation_table[0] = 0;      // NO obstacle
+  cost_translation_table[99] = 253;   // INSCRIBED obstacle
+  cost_translation_table[100] = 254;  // LETHAL obstacle
+  cost_translation_table[static_cast<unsigned char>(-1)] = 255;  // UNKNOWN
+
+  return cost_translation_table;
+}
+
+}  // namespace explore

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 <explore/explore.h>
+#include <random>
+#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
+{
+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<float>("planner_frequency", 1.0);
+  this->declare_parameter<float>("progress_timeout", 30.0);
+  this->declare_parameter<bool>("visualize", false);
+  this->declare_parameter<float>("potential_scale", 1e-3);
+  this->declare_parameter<float>("orientation_scale", 0.0);
+  this->declare_parameter<float>("gain_scale", 1.0);
+  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("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<nav2_msgs::action::NavigateToPose>(
+          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<visualization_msgs::msg::MarkerArray>("explore/"
+                                                                     "frontier"
+                                                                     "s",
+                                                                     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");
+  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<nav2_msgs::action::NavigateToPose>::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<frontier_exploration::Frontier>& 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<visualization_msgs::msg::Marker>& 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<nav2_msgs::action::NavigateToPose>::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<nav2_msgs::action::NavigateToPose>::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<explore::Explore>());  // std::move(std::make_unique)?
+  rclcpp::shutdown();
+  return 0;
+}

m-explore-ros2/explore/src/frontier_search.cpp

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

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 <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();
+}

patches/turtlebot3_office.launch.py

@@ -0,0 +1,68 @@
+#!/usr/bin/env python3
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.conditions import UnlessCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    launch_file_dir = os.path.join(
+        get_package_share_directory('turtlebot3_gazebo'),
+        'launch'
+    )
+    pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
+
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+    x_pose = LaunchConfiguration('x_pose', default='-2.0')
+    y_pose = LaunchConfiguration('y_pose', default='-0.5')
+    headless = LaunchConfiguration('headless', default='false')
+
+    world = os.path.join(
+        get_package_share_directory('turtlebot3_gazebo'),
+        'worlds',
+        'office.world'
+    )
+
+    gzserver_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')
+        ),
+        launch_arguments={'world': world}.items()
+    )
+
+    gzclient_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')
+        ),
+        condition=UnlessCondition(headless)
+    )
+
+    robot_state_publisher_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_file_dir, 'robot_state_publisher.launch.py')
+        ),
+        launch_arguments={'use_sim_time': use_sim_time}.items()
+    )
+
+    spawn_turtlebot_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_file_dir, 'spawn_turtlebot3.launch.py')
+        ),
+        launch_arguments={
+            'x_pose': x_pose,
+            'y_pose': y_pose
+        }.items()
+    )
+
+    ld = LaunchDescription()
+    ld.add_action(gzserver_cmd)
+    ld.add_action(gzclient_cmd)
+    ld.add_action(robot_state_publisher_cmd)
+    ld.add_action(spawn_turtlebot_cmd)
+
+    return ld

run_ab.sh

@@ -0,0 +1,99 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+# One-command runner for:
+#   Terminal A: Gazebo Sim + bridge (+ optional SLAM + RViz)
+#   Terminal B: Nav2 stack
+#
+# Modes:
+#   mapping       -> online SLAM mapping (Localization in RViz is expected to stay inactive)
+#   localization  -> map-based localization/navigation (Localization becomes active)
+#
+# Examples:
+#   ./run_ab.sh mapping
+#   NAV2_MAP_PATH=/abs/path/office_map.yaml ./run_ab.sh localization
+
+ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+MODE="${1:-mapping}"
+ROBOT_MODEL="${ROBOT_MODEL:-waffle}"
+GZ_PARTITION="${GZ_PARTITION:-tb3_office_gz}"
+USE_SIM_TIME="${USE_SIM_TIME:-true}"
+SLEEP_NAV2="${SLEEP_NAV2:-18}"
+WORLD_PATH="${WORLD_PATH:-${ROOT}/turtlebot3_simulations/turtlebot3_gazebo/worlds/office_gz_dartsim.sdf}"
+NAV2_MAP_PATH="${NAV2_MAP_PATH:-${ROOT}/turtlebot3_simulations/turtlebot3_gazebo/map/office_map.yaml}"
+
+set +u
+source /opt/ros/humble/setup.bash
+source "${ROOT}/install/setup.bash"
+set -u
+export TURTLEBOT3_MODEL="${ROBOT_MODEL}"
+
+declare -a CMD_A
+declare -a CMD_B
+
+if [[ "${MODE}" == "mapping" ]]; then
+  CMD_A=(
+    ros2 launch turtlebot3_gazebo turtlebot3_office_gz.launch.py
+    "robot_model:=${ROBOT_MODEL}"
+    "use_sim_time:=${USE_SIM_TIME}"
+    start_slam:=true
+    show_rviz:=true
+    "gz_partition:=${GZ_PARTITION}"
+    gz_world_name:=default
+    "world:=${WORLD_PATH}"
+  )
+  CMD_B=(
+    ros2 launch nav2_bringup navigation_launch.py
+    "use_sim_time:=${USE_SIM_TIME}"
+  )
+elif [[ "${MODE}" == "localization" ]]; then
+  if [[ ! -f "${NAV2_MAP_PATH}" ]]; then
+    echo "[run_ab] ERROR: map yaml not found: ${NAV2_MAP_PATH}" >&2
+    echo "[run_ab] Hint: save map first with map_saver_cli." >&2
+    exit 1
+  fi
+  CMD_A=(
+    ros2 launch turtlebot3_gazebo turtlebot3_office_gz.launch.py
+    "robot_model:=${ROBOT_MODEL}"
+    "use_sim_time:=${USE_SIM_TIME}"
+    start_slam:=false
+    show_rviz:=true
+    "gz_partition:=${GZ_PARTITION}"
+    gz_world_name:=default
+    "world:=${WORLD_PATH}"
+  )
+  CMD_B=(
+    ros2 launch nav2_bringup bringup_launch.py
+    "use_sim_time:=${USE_SIM_TIME}"
+    "map:=${NAV2_MAP_PATH}"
+    slam:=false
+  )
+else
+  echo "[run_ab] ERROR: unknown mode '${MODE}' (use: mapping | localization)" >&2
+  exit 1
+fi
+
+LOG_DIR="${ROOT}/logs/run_ab_$(date +%Y%m%d_%H%M%S)"
+mkdir -p "${LOG_DIR}"
+
+echo "[run_ab] mode=${MODE}"
+echo "[run_ab] logs at: ${LOG_DIR}"
+echo "[run_ab] starting Terminal A..."
+"${CMD_A[@]}" >"${LOG_DIR}/A_sim.log" 2>&1 &
+PID_A=$!
+
+sleep "${SLEEP_NAV2}"
+
+echo "[run_ab] starting Terminal B..."
+"${CMD_B[@]}" >"${LOG_DIR}/B_nav2.log" 2>&1 &
+PID_B=$!
+
+cleanup() {
+  echo "[run_ab] stopping..."
+  kill "${PID_B}" 2>/dev/null || true
+  kill "${PID_A}" 2>/dev/null || true
+}
+trap cleanup EXIT INT TERM
+
+echo "[run_ab] running. Press Ctrl+C to stop both."
+wait "${PID_A}" "${PID_B}"

run_all.sh

@@ -0,0 +1,339 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+# Unified launcher for TurtleBot3 office workflow:
+# - Terminal A: simulation / bridge / (optional SLAM) / RViz
+# - Terminal B: Nav2
+# - Optional C: explore_lite
+# - Optional websocket + gzweb-demo frontend
+#
+# Modes:
+#   mapping       : online SLAM mapping
+#   localization  : map-based localization/navigation
+#
+# Mapping mode interactive commands:
+#   s : save map and switch to localization mode
+#   q : stop all
+#
+# Examples:
+#   ./run_all.sh mapping --explore --gzweb
+#   ./run_all.sh localization --gzweb --map /abs/path/office_map.yaml
+
+ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+MODE="${1:-mapping}"
+shift || true
+
+ROBOT_MODEL="${ROBOT_MODEL:-waffle}"
+USE_SIM_TIME="${USE_SIM_TIME:-true}"
+GZ_PARTITION="${GZ_PARTITION:-tb3_office_gz}"
+WORLD_PATH_DEFAULT="${ROOT}/turtlebot3_simulations/turtlebot3_gazebo/worlds/office_gz_dartsim.sdf"
+MAP_YAML_DEFAULT="${ROOT}/turtlebot3_simulations/turtlebot3_gazebo/map/office_map.yaml"
+MAP_BASENAME_DEFAULT="${ROOT}/turtlebot3_simulations/turtlebot3_gazebo/map/office_map"
+SLEEP_NAV2="${SLEEP_NAV2:-18}"
+EXPLORE_WAIT_TIMEOUT="${EXPLORE_WAIT_TIMEOUT:-420}"
+
+WORLD_PATH="${WORLD_PATH:-$WORLD_PATH_DEFAULT}"
+MAP_YAML="${NAV2_MAP_PATH:-$MAP_YAML_DEFAULT}"
+MAP_BASENAME="${MAP_BASENAME:-$MAP_BASENAME_DEFAULT}"
+
+WITH_EXPLORE=false
+WITH_GZWEB=false
+NO_RVIZ=false
+# 与 --gzweb 同用：只起 gz websocket(9002) 等资源，不起 office 内 npm run dev；页面用镜像内 /opt/gzweb（foundation）
+NO_GZWEB_VITE=false
+
+while [[ $# -gt 0 ]]; do
+  case "$1" in
+    --explore) WITH_EXPLORE=true; shift ;;
+    --gzweb) WITH_GZWEB=true; shift ;;
+    --no-gzweb-vite) NO_GZWEB_VITE=true; shift ;;
+    --no-rviz) NO_RVIZ=true; shift ;;
+    --map) MAP_YAML="$2"; shift 2 ;;
+    --world) WORLD_PATH="$2"; shift 2 ;;
+    --help|-h)
+      cat <<'EOF'
+Usage:
+  ./run_all.sh mapping [--explore] [--gzweb] [--no-gzweb-vite] [--no-rviz] [--world PATH]
+  ./run_all.sh localization [--gzweb] [--no-gzweb-vite] [--no-rviz] [--map PATH] [--world PATH]
+
+  --no-gzweb-vite  需与 --gzweb 同时使用：不启动 workspace 内 gzweb-demo 的 Vite；浏览器用 /opt/gzweb（8000）。
+EOF
+      exit 0
+      ;;
+    *)
+      echo "[run_all] unknown argument: $1" >&2
+      exit 1
+      ;;
+  esac
+done
+
+if [[ "$MODE" != "mapping" && "$MODE" != "localization" ]]; then
+  echo "[run_all] mode must be mapping or localization" >&2
+  exit 1
+fi
+
+if [[ ! -f "$WORLD_PATH" ]]; then
+  echo "[run_all] world not found: $WORLD_PATH" >&2
+  exit 1
+fi
+
+if [[ "$MODE" == "localization" && ! -f "$MAP_YAML" ]]; then
+  echo "[run_all] map yaml not found: $MAP_YAML" >&2
+  exit 1
+fi
+
+set +u
+source /opt/ros/humble/setup.bash
+source "${ROOT}/install/setup.bash"
+set -u
+export TURTLEBOT3_MODEL="$ROBOT_MODEL"
+
+LOG_DIR="${ROOT}/logs/run_all_$(date +%Y%m%d_%H%M%S)"
+mkdir -p "$LOG_DIR"
+mkdir -p "$(dirname "$MAP_BASENAME")"
+
+PIDS=()
+
+start_bg() {
+  local name="$1"; shift
+  local logfile="${LOG_DIR}/${name}.log"
+  echo "[run_all] starting ${name} ..."
+  "$@" >"$logfile" 2>&1 &
+  local pid=$!
+  PIDS+=("$pid")
+  echo "[run_all] ${name} pid=${pid}, log=${logfile}"
+}
+
+cleanup() {
+  echo "[run_all] stopping all processes..."
+  for pid in "${PIDS[@]:-}"; do
+    kill "$pid" 2>/dev/null || true
+  done
+}
+trap cleanup EXIT INT TERM
+
+wait_for_topic() {
+  local topic="$1"
+  local timeout="${2:-60}"
+  local i=0
+  while (( i < timeout )); do
+    if ros2 topic list 2>/dev/null | grep -Fxq "${topic}"; then
+      return 0
+    fi
+    sleep 1
+    ((i+=1))
+  done
+  return 1
+}
+
+wait_for_port() {
+  local port="$1"
+  local timeout="${2:-30}"
+  local i=0
+  while (( i < timeout )); do
+    if ss -ltn 2>/dev/null | grep -q ":${port} "; then
+      return 0
+    fi
+    sleep 1
+    ((i+=1))
+  done
+  return 1
+}
+
+wait_for_explore_stop_log() {
+  local logfile="$1"
+  local timeout="${2:-900}"
+  local i=0
+  while (( i < timeout )); do
+    if [[ -f "$logfile" ]] && grep -Eq "All frontiers traversed/tried out, stopping\\.|Exploration stopped\\." "$logfile"; then
+      return 0
+    fi
+    sleep 1
+    ((i+=1))
+  done
+  return 1
+}
+
+wait_for_explore_stop_log_with_timer() {
+  local logfile="$1"
+  local timeout="${2:-900}"
+  local start_ts
+  start_ts="$(date +%s)"
+  local i=0
+  while (( i < timeout )); do
+    if [[ -f "$logfile" ]] && grep -Eq "All frontiers traversed/tried out, stopping\\.|Exploration stopped\\." "$logfile"; then
+      return 0
+    fi
+    if (( i % 10 == 0 )); then
+      local now_ts elapsed
+      now_ts="$(date +%s)"
+      elapsed=$(( now_ts - start_ts ))
+      echo "[run_all] explore elapsed: ${elapsed}s"
+    fi
+    sleep 1
+    ((i+=1))
+  done
+  return 1
+}
+
+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
+}
+
+show_rviz_arg="show_rviz:=true"
+if [[ "$NO_RVIZ" == true ]]; then
+  show_rviz_arg="show_rviz:=false"
+fi
+
+if [[ "$MODE" == "mapping" ]]; then
+  start_bg "A_sim" \
+    ros2 launch turtlebot3_gazebo turtlebot3_office_gz.launch.py \
+      robot_model:="$ROBOT_MODEL" \
+      use_sim_time:="$USE_SIM_TIME" \
+      start_slam:=true \
+      "$show_rviz_arg" \
+      gz_partition:="$GZ_PARTITION" \
+      gz_world_name:=default \
+      world:="$WORLD_PATH"
+else
+  start_bg "A_sim" \
+    ros2 launch turtlebot3_gazebo turtlebot3_office_gz.launch.py \
+      robot_model:="$ROBOT_MODEL" \
+      use_sim_time:="$USE_SIM_TIME" \
+      start_slam:=false \
+      "$show_rviz_arg" \
+      gz_partition:="$GZ_PARTITION" \
+      gz_world_name:=default \
+      world:="$WORLD_PATH"
+fi
+
+# --gzweb：gz launch(9002) + 可选 office 内 gzweb-demo 的 Vite（5173）。
+# --gzweb --no-gzweb-vite：不起 office Vite；页面用 foundation 已在 /opt/gzweb:8000 起的实例（见 run_all_fix.sh）。
+if [[ "$WITH_GZWEB" == true ]]; then
+  export GZ_PARTITION
+  OFFICE_MODEL_ROOT="${ROOT}/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_office"
+  RESOURCE_PATHS=(
+    "${HOME}/.gazebo/models"
+    "${ROOT}/turtlebot3_simulations/turtlebot3_gazebo/models"
+    "${OFFICE_MODEL_ROOT}"
+  )
+  for d in "${OFFICE_MODEL_ROOT}"/*/materials/scripts "${OFFICE_MODEL_ROOT}"/*/materials/textures; do
+    [[ -d "${d}" ]] && RESOURCE_PATHS+=("${d}")
+  done
+  RESOURCE_PATH_JOINED="$(IFS=:; echo "${RESOURCE_PATHS[*]}")"
+  export GZ_SIM_RESOURCE_PATH="${RESOURCE_PATH_JOINED}"
+  # Keep compatibility with components resolving textures via legacy lookup vars.
+  export GZ_FILE_PATH="${RESOURCE_PATH_JOINED}"
+  export GAZEBO_RESOURCE_PATH="${RESOURCE_PATH_JOINED}"
+
+  if [[ "$NO_GZWEB_VITE" != true ]]; then
+    if [[ ! -d "${ROOT}/gzweb-demo/gzweb-demo/node_modules/gzweb" ]]; then
+      echo "[run_all] gzweb demo dependencies missing, running npm install once..."
+      bash -lc "cd '${ROOT}/gzweb-demo/gzweb-demo' && npm install"
+    fi
+  else
+    echo "[run_all] --no-gzweb-vite: skipping office gzweb-demo npm install; use /opt/gzweb (port 8000) for UI."
+  fi
+
+  kill_port_listeners 9002
+  kill_port_listeners 5173
+
+  echo "[run_all] waiting for /clock before websocket..."
+  if wait_for_topic "/clock" 25; then
+    start_bg "D_websocket" gz launch "${ROOT}/websocket.sdf"
+    if [[ "$NO_GZWEB_VITE" != true ]]; then
+      start_bg "E_gzweb_frontend" bash -lc "cd '${ROOT}/gzweb-demo/gzweb-demo' && npm run dev:host"
+      echo "[run_all] office gzweb-demo Vite: http://localhost:5173"
+    fi
+  else
+    echo "[run_all] WARN: /clock not detected in time; starting websocket (and optional frontend) anyway."
+    start_bg "D_websocket" gz launch "${ROOT}/websocket.sdf"
+    if [[ "$NO_GZWEB_VITE" != true ]]; then
+      start_bg "E_gzweb_frontend" bash -lc "cd '${ROOT}/gzweb-demo/gzweb-demo' && npm run dev:host"
+    fi
+  fi
+
+  if wait_for_port 9002 12; then
+    echo "[run_all] websocket server ready on ws://localhost:9002"
+  else
+    echo "[run_all] ERROR: websocket server failed on 9002; check ${LOG_DIR}/D_websocket.log"
+  fi
+fi
+
+echo "[run_all] waiting ${SLEEP_NAV2}s before starting Nav2..."
+sleep "$SLEEP_NAV2"
+
+if [[ "$MODE" == "mapping" ]]; then
+  start_bg "B_nav2" ros2 launch nav2_bringup navigation_launch.py use_sim_time:="$USE_SIM_TIME"
+else
+  start_bg "B_nav2" \
+    ros2 launch nav2_bringup bringup_launch.py \
+      use_sim_time:="$USE_SIM_TIME" \
+      map:="$MAP_YAML" \
+      slam:=false
+fi
+
+if [[ "$WITH_EXPLORE" == true && "$MODE" == "mapping" ]]; then
+  start_bg "C_explore" ros2 launch explore_lite explore.launch.py use_sim_time:="$USE_SIM_TIME"
+  last_index=$(( ${#PIDS[@]} - 1 ))
+  EXPLORE_PID="${PIDS[$last_index]}"
+  EXPLORE_LOG="${LOG_DIR}/C_explore.log"
+fi
+
+echo
+echo "[run_all] started. Mode: ${MODE}"
+echo "[run_all] logs: ${LOG_DIR}"
+echo
+
+if [[ "$MODE" == "mapping" ]]; then
+  if [[ "$WITH_GZWEB" == true ]]; then
+    if [[ "$NO_GZWEB_VITE" == true ]]; then
+      echo "[run_all] UI: foundation /opt/gzweb on port 8000 (no office Vite)."
+    elif wait_for_port 5173 25; then
+      echo "[run_all] office gzweb-demo ready: http://localhost:5173"
+    else
+      echo "[run_all] WARN: office gzweb frontend not ready yet, check ${LOG_DIR}/E_gzweb_frontend.log"
+    fi
+  fi
+
+  if [[ "${WITH_EXPLORE}" == true && -n "${EXPLORE_PID:-}" ]]; then
+    echo "[run_all] exploration is running (pid=${EXPLORE_PID}), waiting for stop signal (timeout=${EXPLORE_WAIT_TIMEOUT}s)..."
+    if wait_for_explore_stop_log_with_timer "${EXPLORE_LOG}" "${EXPLORE_WAIT_TIMEOUT}"; then
+      echo "[run_all] detected exploration stop from log, finalizing explore process..."
+    else
+      echo "[run_all] WARN: explore stop not detected within timeout; forcing stop and saving current map."
+    fi
+    kill "${EXPLORE_PID}" 2>/dev/null || true
+    wait "${EXPLORE_PID}" 2>/dev/null || true
+    echo "[run_all] explore_lite stopped, now saving map."
+  fi
+
+  if [[ "${WITH_EXPLORE}" == true ]]; then
+    echo "[run_all] auto-saving map to ${MAP_BASENAME}.yaml/.pgm ..."
+    ros2 run nav2_map_server map_saver_cli -f "$MAP_BASENAME" --ros-args -p use_sim_time:="$USE_SIM_TIME"
+    echo "[run_all] map saved successfully:"
+    echo "  - ${MAP_BASENAME}.yaml"
+    echo "  - ${MAP_BASENAME}.pgm"
+    echo "[run_all] mapping workflow completed, exiting now."
+    exit 0
+  fi
+
+  cat <<EOF
+[run_all] Mapping mode notes:
+  - RViz 'Localization: inactive' is normal during SLAM mapping.
+  - You did not enable --explore, so auto-save/auto-exit is disabled.
+  - Press Ctrl+C when you want to stop all.
+EOF
+  wait
+else
+  echo "[run_all] Localization mode: RViz Localization should become active after initial pose."
+  echo "[run_all] Keep this terminal running; Ctrl+C to stop all."
+  wait
+fi

run_all_fix.sh

@@ -0,0 +1,6 @@
+#!/usr/bin/env bash
+# 与 run_all.sh mapping --gzweb 相同，但不起 workspace 内 gzweb-demo 的 npm run dev。
+# 浏览器使用镜像内 foundation 已在 /opt/gzweb（8000）起的页面；本脚本仍起 gz sim、websocket.sdf(9002)、Nav2 等。
+set -euo pipefail
+ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+exec bash "${ROOT}/run_all.sh" mapping --gzweb --no-gzweb-vite

run_all_stop.sh

@@ -0,0 +1,35 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+# Stop all processes started by run_all.sh / run_ab.sh style workflow.
+
+ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+
+echo "[run_all_stop] stopping ROS/Gazebo workflow processes..."
+
+# Launches / nodes
+pkill -f "turtlebot3_office_gz.launch.py" 2>/dev/null || true
+pkill -f "nav2_bringup navigation_launch.py" 2>/dev/null || true
+pkill -f "nav2_bringup bringup_launch.py" 2>/dev/null || true
+pkill -f "explore_lite explore.launch.py" 2>/dev/null || true
+pkill -f "nav2_map_server map_saver_cli" 2>/dev/null || true
+
+# Gazebo websocket and frontend
+pkill -f "${ROOT}/websocket.sdf" 2>/dev/null || true
+pkill -f "vite --host 0.0.0.0 --port 5173" 2>/dev/null || true
+pkill -f "npm run dev:host" 2>/dev/null || true
+
+# Gazebo processes usually started by ros_gz_sim / gz sim
+pkill -f "ros_gz_sim" 2>/dev/null || true
+pkill -f "gz sim" 2>/dev/null || true
+
+# Free common ports if still occupied
+for port in 5173 9002; do
+  pids="$(ss -ltnp 2>/dev/null | grep ":${port} " | sed -E 's/.*pid=([0-9]+).*/\1/' | sort -u)"
+  if [[ -n "${pids}" ]]; then
+    echo "[run_all_stop] killing processes on port ${port}: ${pids}"
+    kill ${pids} 2>/dev/null || true
+  fi
+done
+
+echo "[run_all_stop] done."

run_office_gzweb.sh

@@ -0,0 +1,23 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+# Gazebo Sim 8 + gz-launch WebSocket :9002 for gzweb.
+# Uses office_gz_dartsim.sdf (mesh collisions stripped; dartsim-safe) so the full office scene renders.
+
+ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+WORLD="${ROOT}/turtlebot3_simulations/turtlebot3_gazebo/worlds/office_gz_dartsim.sdf"
+WS_CONFIG="${ROOT}/websocket.sdf"
+PHYSICS="${PHYSICS_ENGINE:-gz-physics-dartsim-plugin}"
+
+export GZ_SIM_RESOURCE_PATH="${HOME}/.gazebo/models:${ROOT}/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_office:${ROOT}/turtlebot3_simulations/turtlebot3_gazebo/models"
+
+gz sim -s -r --physics-engine "${PHYSICS}" "${WORLD}" &
+SIM_PID=$!
+
+cleanup() {
+  kill "${SIM_PID}" 2>/dev/null || true
+}
+trap cleanup EXIT INT TERM
+
+sleep 2
+exec gz launch "${WS_CONFIG}"

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 <scene> into <world> `#65 <https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/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 <https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/53>`_ `#52 <https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/52>`_ `#51 <https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/51>`_ `#50 <https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/50>`_ `#49 <https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/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 <https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/46>`_ from AuTURBO/develop
+  add turtlebot3_autorace world'
+* merged pull request `#48 <https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/48>`_ `#47 <https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/47>`_ `#44 <https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/44>`_ `#42 <https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/42>`_ `#41 <https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/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

turtlebot3_simulations/turtlebot3_gazebo/CMakeLists.txt

@@ -0,0 +1,114 @@
+################################################################################
+# 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
+################################################################################
+find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+
+# Gazebo Classic conflicts with gz-tools2 on many Ubuntu installs. Only probe
+# gazebo after gazebo_ros_pkgs is found, so gz-sim-only systems avoid CMake noise.
+find_package(gazebo_ros_pkgs QUIET)
+set(TURTLEBOT3_GAZEBO_CLASSIC_AVAILABLE FALSE)
+if(gazebo_ros_pkgs_FOUND)
+  find_package(gazebo REQUIRED)
+  set(TURTLEBOT3_GAZEBO_CLASSIC_AVAILABLE TRUE)
+endif()
+
+################################################################################
+# Build (optional Gazebo Classic plugins and turtlebot3_drive)
+################################################################################
+if(TURTLEBOT3_GAZEBO_CLASSIC_AVAILABLE)
+  message(STATUS "turtlebot3_gazebo: building Gazebo Classic plugins and turtlebot3_drive")
+
+  link_directories(${GAZEBO_LIBRARY_DIRS})
+
+  include_directories(
+    include
+    ${GAZEBO_INCLUDE_DIRS}
+  )
+
+  set(dependencies
+    "geometry_msgs"
+    "nav_msgs"
+    "rclcpp"
+    "sensor_msgs"
+    "tf2"
+  )
+
+  set(EXEC_NAME "turtlebot3_drive")
+
+  add_executable(${EXEC_NAME} src/turtlebot3_drive.cpp)
+  ament_target_dependencies(${EXEC_NAME} ${dependencies})
+
+  add_library(traffic_light_plugin SHARED src/traffic_light_plugin.cpp)
+  target_link_libraries(traffic_light_plugin ${GAZEBO_LIBRARIES})
+
+  add_library(traffic_bar_plugin SHARED src/traffic_bar_plugin.cpp)
+  target_link_libraries(traffic_bar_plugin ${GAZEBO_LIBRARIES})
+
+  add_library(obstacle1 SHARED src/obstacle1.cpp)
+  target_link_libraries(obstacle1 ${GAZEBO_LIBRARIES})
+
+  add_library(obstacle2 SHARED src/obstacle2.cpp)
+  target_link_libraries(obstacle2 ${GAZEBO_LIBRARIES})
+
+  add_library(obstacles SHARED src/obstacles.cpp)
+  target_link_libraries(obstacles ${GAZEBO_LIBRARIES})
+
+  install(TARGETS ${EXEC_NAME}
+    DESTINATION lib/${PROJECT_NAME}
+  )
+else()
+  message(STATUS "turtlebot3_gazebo: Gazebo Classic not found — skipping plugins "
+ "(install ros-humble-gazebo-ros-pkgs only if it does not conflict with gz-sim on your system)")
+endif()
+
+################################################################################
+# Install (always: resources for Gazebo Sim / gzweb / ROS 2 launch)
+################################################################################
+install(DIRECTORY launch models rviz urdf worlds config
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+install(PROGRAMS scripts/gen_office_gz_dartsim.py
+  DESTINATION share/${PROJECT_NAME}/scripts
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+################################################################################
+# Macro for ament package
+################################################################################
+ament_export_include_directories(include)
+if(TURTLEBOT3_GAZEBO_CLASSIC_AVAILABLE)
+  ament_export_dependencies(gazebo_ros_pkgs)
+endif()
+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()

turtlebot3_simulations/turtlebot3_gazebo/config/mapper_params_office_gz_sim.yaml

@@ -0,0 +1,65 @@
+# slam_toolbox online_async — tuned for TurtleBot3 + Gazebo Sim GPU lidar (max_range ~6 m).
+# Use with turtlebot3_office_gz.launch.py (start_slam:=true).
+
+slam_toolbox:
+  ros__parameters:
+    solver_plugin: solver_plugins::CeresSolver
+    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
+    ceres_preconditioner: SCHUR_JACOBI
+    ceres_trust_strategy: LEVENBERG_MARQUARDT
+    ceres_dogleg_type: TRADITIONAL_DOGLEG
+    ceres_loss_function: None
+
+    odom_frame: odom
+    map_frame: map
+    base_frame: base_footprint
+    scan_topic: /scan
+    use_map_saver: true
+    mode: mapping
+
+    debug_logging: false
+    throttle_scans: 1
+    transform_publish_period: 0.02
+    map_update_interval: 1.0
+    resolution: 0.05
+    min_laser_range: 0.15
+    max_laser_range: 6.0
+    minimum_time_interval: 0.1
+    transform_timeout: 0.5
+    tf_buffer_duration: 60.0
+    stack_size_to_use: 40000000
+    enable_interactive_mode: true
+
+    use_scan_matching: true
+    use_scan_barycenter: true
+    minimum_travel_distance: 0.1
+    minimum_travel_heading: 0.1
+    scan_buffer_size: 30
+    scan_buffer_maximum_scan_distance: 6.0
+    link_match_minimum_response_fine: 0.1
+    link_scan_maximum_distance: 1.5
+    loop_search_maximum_distance: 3.0
+    do_loop_closing: true
+    loop_match_minimum_chain_size: 10
+    loop_match_maximum_variance_coarse: 3.0
+    loop_match_minimum_response_coarse: 0.35
+    loop_match_minimum_response_fine: 0.45
+
+    correlation_search_space_dimension: 0.5
+    correlation_search_space_resolution: 0.01
+    correlation_search_space_smear_deviation: 0.1
+
+    loop_search_space_dimension: 8.0
+    loop_search_space_resolution: 0.05
+    loop_search_space_smear_deviation: 0.03
+
+    distance_variance_penalty: 0.5
+    angle_variance_penalty: 1.0
+    fine_search_angle_offset: 0.00349
+    coarse_search_angle_offset: 0.349
+    coarse_angle_resolution: 0.0349
+    minimum_angle_penalty: 0.9
+    minimum_distance_penalty: 0.5
+    use_response_expansion: true
+    min_pass_through: 2
+    occupancy_threshold: 0.1

turtlebot3_simulations/turtlebot3_gazebo/include/turtlebot3_gazebo/obstacle1.hpp

@@ -0,0 +1,38 @@
+// Copyright 2012 Open Source Robotics Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Author: Ryan Shim
+
+#ifndef TURTLEBOT3_GAZEBO__OBSTACLE1_HPP_
+#define TURTLEBOT3_GAZEBO__OBSTACLE1_HPP_
+
+#include <ignition/math.hh>
+#include <gazebo/common/common.hh>
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+
+namespace gazebo
+{
+class Obstacle1 : public ModelPlugin
+{
+public:
+  void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) override;
+
+private:
+  physics::ModelPtr model;
+  event::ConnectionPtr updateConnection;
+};
+GZ_REGISTER_MODEL_PLUGIN(Obstacle1);
+}  // namespace gazebo
+#endif  // TURTLEBOT3_GAZEBO__OBSTACLE1_HPP_

turtlebot3_simulations/turtlebot3_gazebo/include/turtlebot3_gazebo/obstacle2.hpp

@@ -0,0 +1,39 @@
+// Copyright 2012 Open Source Robotics Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Author: Ryan Shim
+
+#ifndef TURTLEBOT3_GAZEBO__OBSTACLE2_HPP_
+#define TURTLEBOT3_GAZEBO__OBSTACLE2_HPP_
+
+#include <ignition/math.hh>
+#include <gazebo/common/common.hh>
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+
+namespace gazebo
+{
+class Obstacle2 : public ModelPlugin
+{
+public:
+  Obstacle2() = default;
+  void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) override;
+
+private:
+  physics::ModelPtr model;
+  event::ConnectionPtr updateConnection;
+};
+GZ_REGISTER_MODEL_PLUGIN(Obstacle2);
+}  // namespace gazebo
+#endif  // TURTLEBOT3_GAZEBO__OBSTACLE2_HPP_

turtlebot3_simulations/turtlebot3_gazebo/include/turtlebot3_gazebo/obstacles.hpp

@@ -0,0 +1,41 @@
+// Copyright 2012 Open Source Robotics Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Author: Ryan Shim
+
+#ifndef TURTLEBOT3_GAZEBO__OBSTACLES_HPP_
+#define TURTLEBOT3_GAZEBO__OBSTACLES_HPP_
+
+#include <ignition/math.hh>
+#include <gazebo/common/common.hh>
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+
+#define PI 3.141592
+
+namespace gazebo
+{
+class Obstacles : public ModelPlugin
+{
+public:
+  Obstacles() = default;
+  void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) override;
+
+private:
+  physics::ModelPtr model;
+  event::ConnectionPtr updateConnection;
+};
+GZ_REGISTER_MODEL_PLUGIN(Obstacles);
+}  // namespace gazebo
+#endif  // TURTLEBOT3_GAZEBO__OBSTACLES_HPP_

turtlebot3_simulations/turtlebot3_gazebo/include/turtlebot3_gazebo/traffic_bar_plugin.hpp

@@ -0,0 +1,45 @@
+// Copyright 2025 ROBOTIS CO., LTD.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Author: Hyungyu Kim
+
+#ifndef TURTLEBOT3_GAZEBO__TRAFFIC_BAR_PLUGIN_HPP_
+#define TURTLEBOT3_GAZEBO__TRAFFIC_BAR_PLUGIN_HPP_
+
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+
+namespace gazebo
+{
+class TrafficBar : public ModelPlugin
+{
+public:
+  TrafficBar();
+  void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) override;
+  void OnUpdate();
+
+private:
+  double traffic_cycle;
+  int status;
+
+  common::Time last_time;
+  event::ConnectionPtr update_connection;
+  ignition::math::Pose3d down_pose;
+  ignition::math::Pose3d up_pose;
+  physics::ModelPtr model;
+  physics::WorldPtr world;
+};
+GZ_REGISTER_MODEL_PLUGIN(TrafficBar);
+}  // namespace gazebo
+#endif  // TURTLEBOT3_GAZEBO__TRAFFIC_BAR_PLUGIN_HPP_

turtlebot3_simulations/turtlebot3_gazebo/include/turtlebot3_gazebo/traffic_light_plugin.hpp

@@ -0,0 +1,53 @@
+// Copyright 2025 ROBOTIS CO., LTD.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Author: Hyungyu Kim
+
+#ifndef TURTLEBOT3_GAZEBO__TRAFFIC_LIGHT_PLUGIN_HPP_
+#define TURTLEBOT3_GAZEBO__TRAFFIC_LIGHT_PLUGIN_HPP_
+
+#include <string>
+#include <vector>
+
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+
+namespace gazebo
+{
+class TrafficLight : public ModelPlugin
+{
+public:
+  TrafficLight();
+  ~TrafficLight();
+
+  void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) override;
+  void OnUpdate();
+
+private:
+  double traffic_cycle;
+  int status;
+  std::vector<std::string> textures;
+
+  common::Time last_time;
+  event::ConnectionPtr update_connection;
+  gazebo::transport::NodePtr node;
+  gazebo::msgs::Visual msg;
+  gazebo::transport::PublisherPtr visPub;
+  physics::ModelPtr model;
+  physics::WorldPtr world;
+};
+GZ_REGISTER_MODEL_PLUGIN(TrafficLight);
+}  // namespace gazebo
+
+#endif  // TURTLEBOT3_GAZEBO__TRAFFIC_LIGHT_PLUGIN_HPP_

turtlebot3_simulations/turtlebot3_gazebo/include/turtlebot3_gazebo/turtlebot3_drive.hpp

@@ -0,0 +1,70 @@
+// Copyright 2019 ROBOTIS CO., LTD.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Authors: Taehun Lim (Darby), Ryan Shim
+
+#ifndef TURTLEBOT3_GAZEBO__TURTLEBOT3_DRIVE_HPP_
+#define TURTLEBOT3_GAZEBO__TURTLEBOT3_DRIVE_HPP_
+
+#include <tf2/LinearMath/Matrix3x3.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <geometry_msgs/msg/twist.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+
+#define DEG2RAD (M_PI / 180.0)
+#define RAD2DEG (180.0 / M_PI)
+
+#define CENTER 0
+#define LEFT   1
+#define RIGHT  2
+
+#define LINEAR_VELOCITY  0.3
+#define ANGULAR_VELOCITY 1.5
+
+#define GET_TB3_DIRECTION 0
+#define TB3_DRIVE_FORWARD 1
+#define TB3_RIGHT_TURN    2
+#define TB3_LEFT_TURN     3
+
+class Turtlebot3Drive : public rclcpp::Node
+{
+public:
+  Turtlebot3Drive();
+  ~Turtlebot3Drive();
+
+private:
+  // ROS topic publishers
+  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
+
+  // ROS topic subscribers
+  rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+
+  // Variables
+  double robot_pose_;
+  double prev_robot_pose_;
+  double scan_data_[3];
+
+  // ROS timer
+  rclcpp::TimerBase::SharedPtr update_timer_;
+
+  // Function prototypes
+  void update_callback();
+  void update_cmd_vel(double linear, double angular);
+  void scan_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg);
+  void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg);
+};
+#endif  // TURTLEBOT3_GAZEBO__TURTLEBOT3_DRIVE_HPP_

turtlebot3_simulations/turtlebot3_gazebo/launch/empty_world.launch.py

@@ -0,0 +1,80 @@
+#!/usr/bin/env python3
+#
+# Copyright 2019 ROBOTIS CO., LTD.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# Authors: Joep Tool
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch')
+    pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
+
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+    x_pose = LaunchConfiguration('x_pose', default='0.0')
+    y_pose = LaunchConfiguration('y_pose', default='0.0')
+
+    world = os.path.join(
+        get_package_share_directory('turtlebot3_gazebo'),
+        'worlds',
+        'empty_world.world'
+    )
+
+    gzserver_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')
+        ),
+        launch_arguments={'world': world}.items()
+    )
+
+    gzclient_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')
+        )
+    )
+
+    robot_state_publisher_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_file_dir, 'robot_state_publisher.launch.py')
+        ),
+        launch_arguments={'use_sim_time': use_sim_time}.items()
+    )
+
+    spawn_turtlebot_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_file_dir, 'spawn_turtlebot3.launch.py')
+        ),
+        launch_arguments={
+            'x_pose': x_pose,
+            'y_pose': y_pose
+        }.items()
+    )
+
+    ld = LaunchDescription()
+
+    # Add the commands to the launch description
+    ld.add_action(gzserver_cmd)
+    ld.add_action(gzclient_cmd)
+    ld.add_action(robot_state_publisher_cmd)
+    ld.add_action(spawn_turtlebot_cmd)
+
+    return ld

turtlebot3_simulations/turtlebot3_gazebo/launch/multi_robot.launch.py

@@ -0,0 +1,137 @@
+#!/usr/bin/env python3
+#
+# Copyright 2019 ROBOTIS CO., LTD.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# Authors: Joep Tool, HyunGyu Kim
+
+import os
+import xml.etree.ElementTree as ET
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import GroupAction
+from launch.actions import IncludeLaunchDescription
+from launch.actions import RegisterEventHandler
+from launch.event_handlers import OnShutdown
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import PushRosNamespace
+
+
+def generate_launch_description():
+    TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL']
+
+    number_of_robots = 4
+    namespace = 'TB3'
+    pose = [[-2, -0.5], [0.5, -2], [2, 0.5], [-0.5, 2]]
+    model_folder = 'turtlebot3_' + TURTLEBOT3_MODEL
+    urdf_path = os.path.join(
+        get_package_share_directory('turtlebot3_gazebo'),
+        'models',
+        model_folder,
+        'model.sdf'
+    )
+    save_path = os.path.join(
+        get_package_share_directory('turtlebot3_gazebo'),
+        'models',
+        model_folder,
+        'tmp'
+    )
+    launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch')
+    pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
+
+    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
+
+    world = os.path.join(
+        get_package_share_directory('turtlebot3_gazebo'),
+        'worlds',
+        'turtlebot3_world.world'
+    )
+
+    gzserver_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')
+        ),
+        launch_arguments={'world': world}.items()
+    )
+
+    gzclient_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')
+        )
+    )
+
+    robot_state_publisher_cmd_list = []
+
+    for count in range(number_of_robots):
+        robot_state_publisher_cmd_list.append(
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(launch_file_dir, 'robot_state_publisher.launch.py')
+                ),
+                launch_arguments={
+                    'use_sim_time': use_sim_time,
+                    'frame_prefix': f'{namespace}_{count+1}'
+                    }.items()
+            )
+        )
+
+    spawn_turtlebot_cmd_list = []
+
+    for count in range(number_of_robots):
+        tree = ET.parse(urdf_path)
+        root = tree.getroot()
+        for odom_frame_tag in root.iter('odometry_frame'):
+            odom_frame_tag.text = f'{namespace}_{count+1}/odom'
+        for base_frame_tag in root.iter('robot_base_frame'):
+            base_frame_tag.text = f'{namespace}_{count+1}/base_footprint'
+        for scan_frame_tag in root.iter('frame_name'):
+            scan_frame_tag.text = f'{namespace}_{count+1}/base_scan'
+        urdf_modified = ET.tostring(tree.getroot(), encoding='unicode')
+        urdf_modified = '<?xml version="1.0" ?>\n'+urdf_modified
+        with open(f'{save_path}{count+1}.sdf', 'w') as file:
+            file.write(urdf_modified)
+
+        spawn_turtlebot_cmd_list.append(
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(launch_file_dir, 'multi_spawn_turtlebot3.launch.py')
+                ),
+                launch_arguments={
+                        'x_pose': str(pose[count][0]),
+                        'y_pose': str(pose[count][1]),
+                        'robot_name': f'{TURTLEBOT3_MODEL}_{count+1}',
+                        'namespace': f'{namespace}_{count+1}',
+                        'sdf_path': f'{save_path}{count+1}.sdf'
+                }.items()
+            )
+        )
+
+    ld = LaunchDescription()
+    # Add the commands to the launch description
+    ld.add_action(gzserver_cmd)
+    ld.add_action(gzclient_cmd)
+    ld.add_action(RegisterEventHandler(
+        OnShutdown(
+            on_shutdown=lambda event,
+            context: [os.remove(f'{save_path}{count+1}.sdf') for count in range(number_of_robots)]
+        )
+    ))
+    for count, spawn_turtlebot_cmd in enumerate(spawn_turtlebot_cmd_list, start=1):
+        ld.add_action(GroupAction([PushRosNamespace(f'{namespace}_{count}'),
+                                  robot_state_publisher_cmd_list[count-1],
+                                  spawn_turtlebot_cmd]))
+
+    return ld

turtlebot3_simulations/turtlebot3_gazebo/launch/multi_spawn_turtlebot3.launch.py

@@ -0,0 +1,60 @@
+#!/usr/bin/env python3
+# Copyright 2019 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# Authors: HyunGyu Kim
+
+import os
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL']
+
+    x_pose = LaunchConfiguration('x_pose', default='0.0')
+    y_pose = LaunchConfiguration('y_pose', default='0.0')
+    robot_name = LaunchConfiguration('robot_name', default=TURTLEBOT3_MODEL)
+    namespace = LaunchConfiguration('namespace', default='')
+    sdf_path = LaunchConfiguration('sdf_path', default='')
+
+    declare_x_position_cmd = DeclareLaunchArgument(
+        'x_pose', default_value='0.0',
+        description='Specify namespace of the robot')
+
+    declare_y_position_cmd = DeclareLaunchArgument(
+        'y_pose', default_value='0.0',
+        description='Specify namespace of the robot')
+    start_gazebo_ros_spawner_cmd = Node(
+        package='gazebo_ros',
+        executable='spawn_entity.py',
+        arguments=[
+            '-entity', robot_name,
+            '-file', sdf_path,
+            '-x', x_pose,
+            '-y', y_pose,
+            '-z', '0.01',
+            '-robot_namespace', namespace
+        ],
+        output='screen',
+    )
+
+    ld = LaunchDescription()
+    ld.add_action(declare_x_position_cmd)
+    ld.add_action(declare_y_position_cmd)
+    ld.add_action(start_gazebo_ros_spawner_cmd)
+    return ld

turtlebot3_simulations/turtlebot3_gazebo/launch/robot_state_publisher.launch.py

@@ -0,0 +1,62 @@
+#!/usr/bin/env python3
+#
+# Copyright 2019 ROBOTIS CO., LTD.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# Authors: Darby Lim
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.substitutions import PythonExpression
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL']
+
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+    urdf_file_name = 'turtlebot3_' + TURTLEBOT3_MODEL + '.urdf'
+    frame_prefix = LaunchConfiguration('frame_prefix', default='')
+
+    print('urdf_file_name : {}'.format(urdf_file_name))
+
+    urdf_path = os.path.join(
+        get_package_share_directory('turtlebot3_gazebo'),
+        'urdf',
+        urdf_file_name)
+
+    with open(urdf_path, 'r') as infp:
+        robot_desc = infp.read()
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'use_sim_time',
+            default_value='false',
+            description='Use simulation (Gazebo) clock if true'),
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            name='robot_state_publisher',
+            output='screen',
+            parameters=[{
+                'use_sim_time': use_sim_time,
+                'robot_description': robot_desc,
+                'frame_prefix': PythonExpression(["'", frame_prefix, "/'"])
+            }],
+        ),
+    ])

turtlebot3_simulations/turtlebot3_gazebo/launch/spawn_turtlebot3.launch.py

@@ -0,0 +1,70 @@
+# Copyright 2019 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    # Get the urdf file
+    TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL']
+    model_folder = 'turtlebot3_' + TURTLEBOT3_MODEL
+    urdf_path = os.path.join(
+        get_package_share_directory('turtlebot3_gazebo'),
+        'models',
+        model_folder,
+        'model.sdf'
+    )
+
+    # Launch configuration variables specific to simulation
+    x_pose = LaunchConfiguration('x_pose', default='3.0')
+    y_pose = LaunchConfiguration('y_pose', default='3.0')
+
+    # Declare the launch arguments
+    declare_x_position_cmd = DeclareLaunchArgument(
+        'x_pose', default_value='3.0',
+        description='Specify namespace of the robot')
+
+    declare_y_position_cmd = DeclareLaunchArgument(
+        'y_pose', default_value='3.0',
+        description='Specify namespace of the robot')
+
+    start_gazebo_ros_spawner_cmd = Node(
+        package='gazebo_ros',
+        executable='spawn_entity.py',
+        arguments=[
+            '-entity', TURTLEBOT3_MODEL,
+            '-file', urdf_path,
+            '-x', x_pose,
+            '-y', y_pose,
+            '-z', '0.01'
+        ],
+        output='screen',
+    )
+
+    ld = LaunchDescription()
+
+    # Declare the launch options
+    ld.add_action(declare_x_position_cmd)
+    ld.add_action(declare_y_position_cmd)
+
+    # Add any conditioned actions
+    ld.add_action(start_gazebo_ros_spawner_cmd)
+
+    return ld

turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_autorace_2020.launch.py

@@ -0,0 +1,77 @@
+#!/usr/bin/env python3
+#
+# Copyright 2019 ROBOTIS CO., LTD.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# Authors: Joep Tool, Hyungyu Kim
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch')
+    pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
+
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+    x_pose = LaunchConfiguration('x_pose', default='0.8')
+    y_pose = LaunchConfiguration('y_pose', default='-1.747')
+
+    world = os.path.join(
+        get_package_share_directory('turtlebot3_gazebo'),
+        'worlds',
+        'turtlebot3_autorace_2020.world'
+    )
+
+    gzserver_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')
+        ),
+        launch_arguments={'world': world}.items()
+    )
+
+    gzclient_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')
+        )
+    )
+
+    robot_state_publisher_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_file_dir, 'robot_state_publisher.launch.py')
+        ),
+        launch_arguments={'use_sim_time': use_sim_time}.items()
+    )
+
+    spawn_turtlebot_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_file_dir, 'spawn_turtlebot3.launch.py')
+        ),
+        launch_arguments={
+            'x_pose': x_pose,
+            'y_pose': y_pose
+        }.items()
+    )
+
+    ld = LaunchDescription()
+    ld.add_action(gzserver_cmd)
+    ld.add_action(gzclient_cmd)
+    ld.add_action(robot_state_publisher_cmd)
+    ld.add_action(spawn_turtlebot_cmd)
+    return ld

turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_dqn_stage1.launch.py

@@ -0,0 +1,80 @@
+#!/usr/bin/env python3
+#
+# Copyright 2019 ROBOTIS CO., LTD.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# Authors: Joep Tool
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch')
+    pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
+
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+    x_pose = LaunchConfiguration('x_pose', default='0.0')
+    y_pose = LaunchConfiguration('y_pose', default='0.0')
+
+    world = os.path.join(
+        get_package_share_directory('turtlebot3_gazebo'),
+        'worlds',
+        'turtlebot3_dqn_stage1.world'
+    )
+
+    gzserver_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')
+        ),
+        launch_arguments={'world': world}.items()
+    )
+
+    gzclient_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')
+        )
+    )
+
+    robot_state_publisher_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_file_dir, 'robot_state_publisher.launch.py')
+        ),
+        launch_arguments={'use_sim_time': use_sim_time}.items()
+    )
+
+    spawn_turtlebot_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_file_dir, 'spawn_turtlebot3.launch.py')
+        ),
+        launch_arguments={
+            'x_pose': x_pose,
+            'y_pose': y_pose
+        }.items()
+    )
+
+    ld = LaunchDescription()
+
+    # Add the commands to the launch description
+    ld.add_action(gzserver_cmd)
+    ld.add_action(gzclient_cmd)
+    ld.add_action(robot_state_publisher_cmd)
+    ld.add_action(spawn_turtlebot_cmd)
+
+    return ld

turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_dqn_stage2.launch.py

@@ -0,0 +1,80 @@
+#!/usr/bin/env python3
+#
+# Copyright 2019 ROBOTIS CO., LTD.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# Authors: Joep Tool
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch')
+    pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
+
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+    x_pose = LaunchConfiguration('x_pose', default='0.0')
+    y_pose = LaunchConfiguration('y_pose', default='0.0')
+
+    world = os.path.join(
+        get_package_share_directory('turtlebot3_gazebo'),
+        'worlds',
+        'turtlebot3_dqn_stage2.world'
+    )
+
+    gzserver_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')
+        ),
+        launch_arguments={'world': world}.items()
+    )
+
+    gzclient_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')
+        )
+    )
+
+    robot_state_publisher_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_file_dir, 'robot_state_publisher.launch.py')
+        ),
+        launch_arguments={'use_sim_time': use_sim_time}.items()
+    )
+
+    spawn_turtlebot_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_file_dir, 'spawn_turtlebot3.launch.py')
+        ),
+        launch_arguments={
+            'x_pose': x_pose,
+            'y_pose': y_pose
+        }.items()
+    )
+
+    ld = LaunchDescription()
+
+    # Add the commands to the launch description
+    ld.add_action(gzserver_cmd)
+    ld.add_action(gzclient_cmd)
+    ld.add_action(robot_state_publisher_cmd)
+    ld.add_action(spawn_turtlebot_cmd)
+
+    return ld

turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_dqn_stage3.launch.py

@@ -0,0 +1,80 @@
+#!/usr/bin/env python3
+#
+# Copyright 2019 ROBOTIS CO., LTD.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# Authors: Joep Tool
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch')
+    pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
+
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+    x_pose = LaunchConfiguration('x_pose', default='0.0')
+    y_pose = LaunchConfiguration('y_pose', default='0.0')
+
+    world = os.path.join(
+        get_package_share_directory('turtlebot3_gazebo'),
+        'worlds',
+        'turtlebot3_dqn_stage3.world'
+    )
+
+    gzserver_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')
+        ),
+        launch_arguments={'world': world}.items()
+    )
+
+    gzclient_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')
+        )
+    )
+
+    robot_state_publisher_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_file_dir, 'robot_state_publisher.launch.py')
+        ),
+        launch_arguments={'use_sim_time': use_sim_time}.items()
+    )
+
+    spawn_turtlebot_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_file_dir, 'spawn_turtlebot3.launch.py')
+        ),
+        launch_arguments={
+            'x_pose': x_pose,
+            'y_pose': y_pose
+        }.items()
+    )
+
+    ld = LaunchDescription()
+
+    # Add the commands to the launch description
+    ld.add_action(gzserver_cmd)
+    ld.add_action(gzclient_cmd)
+    ld.add_action(robot_state_publisher_cmd)
+    ld.add_action(spawn_turtlebot_cmd)
+
+    return ld

turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_dqn_stage4.launch.py

@@ -0,0 +1,80 @@
+#!/usr/bin/env python3
+#
+# Copyright 2019 ROBOTIS CO., LTD.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# Authors: Joep Tool
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch')
+    pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
+
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+    x_pose = LaunchConfiguration('x_pose', default='0.0')
+    y_pose = LaunchConfiguration('y_pose', default='0.0')
+
+    world = os.path.join(
+        get_package_share_directory('turtlebot3_gazebo'),
+        'worlds',
+        'turtlebot3_dqn_stage4.world'
+    )
+
+    gzserver_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')
+        ),
+        launch_arguments={'world': world}.items()
+    )
+
+    gzclient_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')
+        )
+    )
+
+    robot_state_publisher_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_file_dir, 'robot_state_publisher.launch.py')
+        ),
+        launch_arguments={'use_sim_time': use_sim_time}.items()
+    )
+
+    spawn_turtlebot_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_file_dir, 'spawn_turtlebot3.launch.py')
+        ),
+        launch_arguments={
+            'x_pose': x_pose,
+            'y_pose': y_pose
+        }.items()
+    )
+
+    ld = LaunchDescription()
+
+    # Add the commands to the launch description
+    ld.add_action(gzserver_cmd)
+    ld.add_action(gzclient_cmd)
+    ld.add_action(robot_state_publisher_cmd)
+    ld.add_action(spawn_turtlebot_cmd)
+
+    return ld

turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_house.launch.py

@@ -0,0 +1,80 @@
+#!/usr/bin/env python3
+#
+# Copyright 2019 ROBOTIS CO., LTD.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# Authors: Joep Tool
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch')
+    pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
+
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+    x_pose = LaunchConfiguration('x_pose', default='-2.0')
+    y_pose = LaunchConfiguration('y_pose', default='-0.5')
+
+    world = os.path.join(
+        get_package_share_directory('turtlebot3_gazebo'),
+        'worlds',
+        'turtlebot3_house.world'
+    )
+
+    gzserver_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')
+        ),
+        launch_arguments={'world': world}.items()
+    )
+
+    gzclient_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')
+        )
+    )
+
+    robot_state_publisher_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_file_dir, 'robot_state_publisher.launch.py')
+        ),
+        launch_arguments={'use_sim_time': use_sim_time}.items()
+    )
+
+    spawn_turtlebot_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_file_dir, 'spawn_turtlebot3.launch.py')
+        ),
+        launch_arguments={
+            'x_pose': x_pose,
+            'y_pose': y_pose
+        }.items()
+    )
+
+    ld = LaunchDescription()
+
+    # Add the commands to the launch description
+    ld.add_action(gzserver_cmd)
+    ld.add_action(gzclient_cmd)
+    ld.add_action(robot_state_publisher_cmd)
+    ld.add_action(spawn_turtlebot_cmd)
+
+    return ld

turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_office.launch.py

@@ -0,0 +1,80 @@
+#!/usr/bin/env python3
+#
+# Copyright 2019 ROBOTIS CO., LTD.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# Authors: Joep Tool
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch')
+    pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
+
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+    x_pose = LaunchConfiguration('x_pose', default='-2.0')
+    y_pose = LaunchConfiguration('y_pose', default='-0.5')
+
+    world = os.path.join(
+        get_package_share_directory('turtlebot3_gazebo'),
+        'worlds',
+        'office.world'  # office_world.world
+    )
+
+    gzserver_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')
+        ),
+        launch_arguments={'world': world}.items()
+    )
+
+    gzclient_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')
+        )
+    )
+
+    robot_state_publisher_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_file_dir, 'robot_state_publisher.launch.py')
+        ),
+        launch_arguments={'use_sim_time': use_sim_time}.items()
+    )
+
+    spawn_turtlebot_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_file_dir, 'spawn_turtlebot3.launch.py')
+        ),
+        launch_arguments={
+            'x_pose': x_pose,
+            'y_pose': y_pose
+        }.items()
+    )
+
+    ld = LaunchDescription()
+
+    # Add the commands to the launch description
+    ld.add_action(gzserver_cmd)
+    ld.add_action(gzclient_cmd)
+    ld.add_action(robot_state_publisher_cmd)
+    ld.add_action(spawn_turtlebot_cmd)
+
+    return ld

turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_office_gz.launch.py

@@ -0,0 +1,310 @@
+#!/usr/bin/env python3
+# Copyright 2019 ROBOTIS CO., LTD.
+# SPDX-License-Identifier: Apache-2.0
+#
+# Gazebo Sim 8 (Harmonic) + ROS 2 Humble — TurtleBot3 办公室 Task1 链路：
+# 默认加载 office_gz_dartsim.sdf（已去除网格碰撞，dartsim 稳定）+ GPU 激光 + Nav2/SLAM 所需话题。
+
+import os
+from pathlib import Path
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    LogInfo,
+    OpaqueFunction,
+    SetEnvironmentVariable,
+    TimerAction,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+
+
+def _turtlebot3_gazebo_share():
+    try:
+        return get_package_share_directory('turtlebot3_gazebo')
+    except LookupError:
+        return str(Path(__file__).resolve().parent.parent)
+
+
+def _launch_setup(context, *args, **kwargs):
+    pkg_share = _turtlebot3_gazebo_share()
+    pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
+
+    world = context.launch_configurations['world']
+    gz_partition = context.launch_configurations['gz_partition']
+    world_name = context.launch_configurations['gz_world_name']
+    physics = context.launch_configurations['physics_engine']
+    headless = context.launch_configurations['headless'].lower() == 'true'
+    x_pose = context.launch_configurations['x_pose']
+    y_pose = context.launch_configurations['y_pose']
+    use_sim_time = context.launch_configurations['use_sim_time']
+    robot_model = context.launch_configurations['robot_model']
+
+    model_gz = os.path.abspath(
+        os.path.join(pkg_share, 'models', f'turtlebot3_{robot_model}', 'model_gz.sdf'))
+    if not os.path.isfile(model_gz):
+        raise RuntimeError(
+            f'Missing Gazebo Sim model: {model_gz}. Supported robot_model: burger, waffle.'
+        )
+
+    world_abs = os.path.abspath(os.path.expanduser(world))
+    if not os.path.isfile(world_abs):
+        raise RuntimeError(
+            f'World file not found: {world_abs}. '
+            'Build/install turtlebot3_gazebo or pass world:=/absolute/path/office_gz_dartsim.sdf'
+        )
+
+    gz_entity = f'turtlebot3_{robot_model}'
+    gz_prefix = '-s ' if headless else ''
+    gz_args = f'{gz_prefix}-r --physics-engine {physics} {world_abs}'
+
+    gz_sim = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')),
+        launch_arguments={'gz_args': gz_args}.items(),
+    )
+
+    urdf_name = f'turtlebot3_{robot_model}.urdf'
+    urdf_path = os.path.join(pkg_share, 'urdf', urdf_name)
+    with open(urdf_path, 'r', encoding='utf-8') as urdf_file:
+        robot_desc = urdf_file.read()
+
+    robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        output='screen',
+        parameters=[{
+            'use_sim_time': use_sim_time.lower() == 'true',
+            'robot_description': robot_desc,
+        }],
+    )
+
+    joint_gz = f'/world/{world_name}/model/{gz_entity}/joint_state'
+    cmd_topic = f'/model/{gz_entity}/cmd_vel'
+    odom_topic = f'/model/{gz_entity}/odometry'
+    # DiffDrive publishes odom->base_footprint as gz.msgs.Pose_V on this topic.
+    tf_topic = f'/model/{gz_entity}/tf'
+
+    # Gazebo sometimes publishes LaserScan frame_id as "<model>/base_scan/hls_lfcd_lds" even with
+    # gz_frame_id; SLAM/RViz expect "base_scan" (URDF). Identity tie fixes TF without changing hits.
+    gz_lidar_frame = f'{gz_entity}/base_scan/hls_lfcd_lds'
+    lidar_frame_broadcaster = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        name='gz_lidar_frame_align',
+        arguments=[
+            '--frame-id', 'base_scan',
+            '--child-frame-id', gz_lidar_frame,
+        ],
+        parameters=[{'use_sim_time': use_sim_time.lower() == 'true'}],
+    )
+
+    bridge = Node(
+        package='ros_gz_bridge',
+        executable='parameter_bridge',
+        arguments=[
+            '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock',
+            f'{cmd_topic}@geometry_msgs/msg/Twist@gz.msgs.Twist',
+            f'{odom_topic}@nav_msgs/msg/Odometry@gz.msgs.Odometry',
+            f'{joint_gz}@sensor_msgs/msg/JointState[gz.msgs.Model',
+            f'{tf_topic}@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V',
+            '/scan@sensor_msgs/msg/LaserScan@gz.msgs.LaserScan',
+        ],
+        remappings=[
+            (joint_gz, '/joint_states'),
+            (tf_topic, '/tf'),
+            (cmd_topic, '/cmd_vel'),
+            (odom_topic, '/odom'),
+        ],
+        parameters=[{
+            f'qos_overrides.{cmd_topic}.subscriber.reliability': 'reliable',
+        }],
+        output='screen',
+    )
+
+    spawn = Node(
+        package='ros_gz_sim',
+        executable='create',
+        arguments=[
+            '-world', world_name,
+            '-file', model_gz,
+            '-name', gz_entity,
+            '-x', x_pose,
+            '-y', y_pose,
+            '-z', '0.05',
+        ],
+        output='screen',
+    )
+
+    delayed_spawn = TimerAction(period=12.0, actions=[spawn])
+
+    set_gz_partition = SetEnvironmentVariable(name='GZ_PARTITION', value=gz_partition)
+
+    set_gz_res = SetEnvironmentVariable(
+        name='GZ_SIM_RESOURCE_PATH',
+        value=os.pathsep.join([
+            os.path.join(pkg_share, 'models', 'turtlebot3_office'),
+            os.path.join(pkg_share, 'models'),
+            os.path.join(Path.home(), '.gazebo', 'models'),
+        ]),
+    )
+
+    log_start = LogInfo(msg=[
+        '[turtlebot3_office_gz] Isolated GZ_PARTITION='
+        + gz_partition
+        + ' (close other gz sim windows or they stay on the default partition). World='
+        + world_abs,
+    ])
+
+    actions = [
+        set_gz_partition,
+        set_gz_res,
+        log_start,
+        robot_state_publisher,
+        lidar_frame_broadcaster,
+        gz_sim,
+        bridge,
+        delayed_spawn,
+    ]
+
+    start_slam = context.launch_configurations['start_slam'].lower() == 'true'
+    if start_slam:
+        try:
+            slam_pkg = get_package_share_directory('slam_toolbox')
+        except LookupError:
+            slam_pkg = ''
+        if slam_pkg:
+            slam_launch = os.path.join(slam_pkg, 'launch', 'online_async_launch.py')
+            slam_params = context.launch_configurations['slam_params_file']
+            if not os.path.isfile(slam_params):
+                raise RuntimeError(
+                    f'slam_params_file not found: {slam_params} '
+                    '(install turtlebot3_gazebo + config, or pass slam_params_file:=...)'
+                )
+            actions.append(
+                TimerAction(
+                    period=14.0,
+                    actions=[
+                        LogInfo(msg=[
+                            '[turtlebot3_office_gz] Starting slam_toolbox (map frame will appear for RViz).',
+                        ]),
+                        IncludeLaunchDescription(
+                            PythonLaunchDescriptionSource(slam_launch),
+                            launch_arguments={
+                                'use_sim_time': context.launch_configurations['use_sim_time'],
+                                'slam_params_file': slam_params,
+                            }.items(),
+                        ),
+                    ],
+                ))
+        else:
+            actions.append(
+                LogInfo(msg=[
+                    '[turtlebot3_office_gz] start_slam=true but slam_toolbox package not found '
+                    '(install ros-humble-slam-toolbox).',
+                ]))
+
+    if context.launch_configurations['show_rviz'].lower() == 'true':
+        try:
+            nav2_share = get_package_share_directory('nav2_bringup')
+            rviz_cfg = os.path.join(nav2_share, 'rviz', 'nav2_default_view.rviz')
+        except LookupError:
+            rviz_cfg = ''
+        if rviz_cfg and os.path.isfile(rviz_cfg):
+            rviz_node = Node(
+                package='rviz2',
+                executable='rviz2',
+                name='rviz2',
+                arguments=['-d', rviz_cfg],
+                parameters=[{'use_sim_time': use_sim_time.lower() == 'true'}],
+                output='screen',
+            )
+            # RViz fixed frame is "map": wait for slam_toolbox; otherwise scans pile up and drop.
+            if start_slam:
+                actions.append(
+                    TimerAction(
+                        period=20.0,
+                        actions=[
+                            LogInfo(msg=[
+                                '[turtlebot3_office_gz] Starting RViz (after SLAM). Drive the robot to map the office.',
+                            ]),
+                            rviz_node,
+                        ],
+                    ))
+            else:
+                actions.append(rviz_node)
+        else:
+            actions.append(
+                LogInfo(msg=[
+                    '[turtlebot3_office_gz] show_rviz=true but nav2_bringup not found or '
+                    'missing nav2_default_view.rviz — install ros-humble-nav2-bringup or '
+                    'run rviz2 manually.',
+                ]))
+
+    return actions
+
+
+def generate_launch_description():
+    pkg_share = _turtlebot3_gazebo_share()
+    default_world = os.path.join(pkg_share, 'worlds', 'office_gz_dartsim.sdf')
+    default_slam_params = os.path.join(pkg_share, 'config', 'mapper_params_office_gz_sim.yaml')
+    default_robot = os.environ.get('TURTLEBOT3_MODEL', 'burger')
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'world',
+            default_value=default_world,
+            description='SDF world file (must match gz_world_name in the file).',
+        ),
+        DeclareLaunchArgument(
+            'gz_partition',
+            default_value='tb3_office_gz',
+            description='Unique Gazebo transport partition so this sim does not attach to another gz sim '
+            '(e.g. tutorial playground / gpu_lidar) on the same machine.',
+        ),
+        DeclareLaunchArgument(
+            'gz_world_name',
+            default_value='default',
+            description='<world name="..."> (office_gz_dartsim.sdf → default).',
+        ),
+        DeclareLaunchArgument(
+            'physics_engine',
+            default_value='gz-physics-dartsim-plugin',
+            description='Physics engine plugin for gz-sim.',
+        ),
+        DeclareLaunchArgument(
+            'headless',
+            default_value='false',
+            description='If true, run gz sim server only (-s).',
+        ),
+        DeclareLaunchArgument(
+            'robot_model',
+            default_value=default_robot,
+            description='TurtleBot3 variant: burger or waffle (must match TURTLEBOT3_MODEL).',
+        ),
+        DeclareLaunchArgument('x_pose', default_value='0.0'),
+        DeclareLaunchArgument('y_pose', default_value='0.0'),
+        DeclareLaunchArgument('use_sim_time', default_value='true'),
+        DeclareLaunchArgument(
+            'show_rviz',
+            default_value='true',
+            description='If true, start RViz2 with nav2_bringup default config (needs ros-humble-nav2-bringup). '
+            'Set false when RViz is started separately (e.g. Docker run_task1.sh).',
+        ),
+        DeclareLaunchArgument(
+            'start_slam',
+            default_value='true',
+            description='If true, start slam_toolbox online_async after the robot spawns (recommended for mapping). '
+            'Set false if you launch SLAM separately (e.g. Docker run_task1.sh).',
+        ),
+        DeclareLaunchArgument(
+            'slam_params_file',
+            default_value=default_slam_params,
+            description='slam_toolbox YAML (office + gz-sim lidar 6 m).',
+        ),
+        OpaqueFunction(function=_launch_setup),
+    ])

turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_world.launch.py

@@ -0,0 +1,80 @@
+#!/usr/bin/env python3
+#
+# Copyright 2019 ROBOTIS CO., LTD.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# Authors: Joep Tool
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch')
+    pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
+
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+    x_pose = LaunchConfiguration('x_pose', default='-2.0')
+    y_pose = LaunchConfiguration('y_pose', default='-0.5')
+
+    world = os.path.join(
+        get_package_share_directory('turtlebot3_gazebo'),
+        'worlds',
+        'turtlebot3_world.world'
+    )
+
+    gzserver_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')
+        ),
+        launch_arguments={'world': world}.items()
+    )
+
+    gzclient_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')
+        )
+    )
+
+    robot_state_publisher_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_file_dir, 'robot_state_publisher.launch.py')
+        ),
+        launch_arguments={'use_sim_time': use_sim_time}.items()
+    )
+
+    spawn_turtlebot_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_file_dir, 'spawn_turtlebot3.launch.py')
+        ),
+        launch_arguments={
+            'x_pose': x_pose,
+            'y_pose': y_pose
+        }.items()
+    )
+
+    ld = LaunchDescription()
+
+    # Add the commands to the launch description
+    ld.add_action(gzserver_cmd)
+    ld.add_action(gzclient_cmd)
+    ld.add_action(robot_state_publisher_cmd)
+    ld.add_action(spawn_turtlebot_cmd)
+
+    return ld

turtlebot3_simulations/turtlebot3_gazebo/map/office_map.yaml

@@ -0,0 +1,7 @@
+image: office_map.pgm
+mode: trinary
+resolution: 0.05
+origin: [-5.73, -5.78, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25

turtlebot3_simulations/turtlebot3_gazebo/map/office_map_new.yaml

@@ -0,0 +1,7 @@
+image: office_map_new.pgm
+mode: trinary
+resolution: 0.05
+origin: [-5.71, -2.7, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25

turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/checker/materials/scripts/checker.material

@@ -0,0 +1,14 @@
+material checker
+{
+  technique
+  {
+    pass
+    {
+      texture_unit
+      {
+        texture checker.png
+      }
+    }
+  }
+}
+

turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/checker/model.config

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>checker</name>
+  <version>1.0</version>
+  <sdf version='1.6'>model.sdf</sdf>
+
+  <author>
+    <name>Gilbert</name>
+    <email>kkjong@robotis.com</email>
+  </author>
+
+  <description>
+    Model with links of simple shapes and texture applied.
+  </description>
+</model>

turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/checker/model.sdf

@@ -0,0 +1,30 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <model name="checker">
+    <static>true</static>
+    <link name="box">
+      <pose>0 0 0 0 0 0</pose>
+      <collision name="collision">
+        <geometry>
+          <box>
+            <size>0.02 1 1</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="visual">
+        <geometry>
+          <box>
+            <size>0.02 1 1</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>model://turtlebot3_autorace_2020/checker/materials/scripts</uri>
+            <uri>model://turtlebot3_autorace_2020/checker/materials/textures</uri>
+            <name>checker</name>
+          </script>
+        </material>
+      </visual>
+    </link>
+  </model>
+</sdf>

turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/course/materials/scripts/course.material

@@ -0,0 +1,14 @@
+material course
+{
+  technique
+  {
+    pass
+    {
+      texture_unit
+      {
+        texture course.png
+      }
+    }
+  }
+}
+

turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/course/model.config

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>course</name>
+  <version>1.0</version>
+  <sdf version="1.6">model.sdf</sdf>
+  <author>
+    <name>Gilbert</name>
+    <email>kkjong@robotis.com</email>
+  </author>
+
+  <description>
+    A simple textured ground plane
+  </description>
+</model>
+

turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/course/model.sdf

@@ -0,0 +1,41 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <model name="course">
+    <static>true</static>
+    <link name="course_link">
+      <collision name="course_collision">
+        <geometry>
+          <plane>
+            <normal>0 0 1</normal>
+            <size>4 4</size>
+          </plane>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>100</mu>
+              <mu2>50</mu2>
+            </ode>
+          </friction>
+        </surface>
+      </collision>
+      <visual name="course_visual">
+        <cast_shadows>false</cast_shadows>
+        <geometry>
+	        <plane>
+            <normal>0 0 1</normal>
+            <size>4 4</size>
+          </plane>
+        </geometry>
+	      <material>
+          <script>
+            <uri>model://turtlebot3_autorace_2020/course/materials/scripts</uri>
+            <uri>model://turtlebot3_autorace_2020/course/materials/textures</uri>
+            <name>course</name>
+          </script>
+        </material>
+      </visual>
+    </link>
+  </model>
+</sdf>
+

turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_bar/materials/scripts/traffic_bar.material

@@ -0,0 +1,14 @@
+material traffic_bar
+{
+  technique
+  {
+    pass
+    {
+      texture_unit
+      {
+        texture traffic_bar.png
+      }
+    }
+  }
+}
+

turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_bar/model.config

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>traffic_bar</name>
+  <version>1.0</version>
+  <sdf version='1.6'>model.sdf</sdf>
+
+  <author>
+    <name>Hyunok Lee</name>
+    <email>hyunokhyunok@naver.com</email>
+    <name>Hyungyu Kim</name>
+    <email>kimhg@robotis.com</email>
+  </author>
+
+  <description>
+    Model with links of simple shapes and texture applied.
+  </description>
+</model>

turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_bar/model.sdf

@@ -0,0 +1,32 @@
+<?xml version='1.0'?>
+<sdf version="1.6">
+  <model name="traffic_bar">
+    <static>false</static>
+    <link name="traffic_bar">
+        <pose> 0 0 0 0 0 0</pose>
+        <collision name="collision">
+          <geometry>
+            <box>
+              <size>0.3 0.02 0.05</size>
+            </box>
+         </geometry>
+        </collision>
+        <visual name="visual">
+        <geometry>
+          <box>
+            <size>0.3 0.02 0.05</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>model://turtlebot3_autorace_2020/traffic_bar/materials/scripts</uri>
+            <uri>model://turtlebot3_autorace_2020/traffic_bar/materials/textures</uri>
+            <name>traffic_bar</name>
+          </script>
+        </material>
+      </visual>
+    </link>
+    <plugin name="traffic_bar_plugin" filename="libtraffic_bar_plugin.so">
+    </plugin>   
+  </model>
+</sdf>

turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_construction/materials/scripts/traffic_construction.material

@@ -0,0 +1,14 @@
+material traffic_construction
+{
+  technique
+  {
+    pass
+    {
+      texture_unit
+      {
+        texture traffic_construction.png
+      }
+    }
+  }
+}
+

turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_construction/model.config

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>traffic_construction</name>
+  <version>1.0</version>
+  <sdf version='1.6'>model.sdf</sdf>
+
+  <author>
+    <name>Hyunok Lee</name>
+    <email>hyunokhyunok@naver.com</email>
+  </author>
+
+  <description>
+    Model with links of simple shapes and texture applied.
+  </description>
+</model>

turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_construction/model.sdf

@@ -0,0 +1,30 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <model name="traffic_construction">
+    <static>true</static>
+    <link name="box">
+      <pose>0 0 0 0 0 0</pose>
+      <collision name="collision">
+        <geometry>
+          <box>
+            <size>0.12 0.025 0.25</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="visual">
+        <geometry>
+          <box>
+            <size>0.12 0.025 0.25</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>model://turtlebot3_autorace_2020/traffic_construction/materials/scripts</uri>
+            <uri>model://turtlebot3_autorace_2020/traffic_construction/materials/textures</uri>
+            <name>traffic_construction</name>
+          </script>
+        </material>
+      </visual>
+    </link>    
+  </model>
+</sdf>

turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_intersection/materials/scripts/traffic_intersection.material

@@ -0,0 +1,14 @@
+material traffic_intersection
+{
+  technique
+  {
+    pass
+    {
+      texture_unit
+      {
+        texture traffic_intersection.png
+      }
+    }
+  }
+}
+

turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_intersection/model.config

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>traffic_intersection</name>
+  <version>1.0</version>
+  <sdf version='1.6'>model.sdf</sdf>
+
+  <author>
+    <name>Hyunok Lee</name>
+    <email>hyunokhyunok@naver.com</email>
+  </author>
+
+  <description>
+    Model with links of simple shapes and texture applied.
+  </description>
+</model>

turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_intersection/model.sdf

@@ -0,0 +1,30 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <model name="traffic_intersection">
+    <static>true</static>
+    <link name="box">
+      <pose>0 0 0 0 0 0</pose>
+      <collision name="collision">
+        <geometry>
+          <box>
+            <size>0.12 0.025 0.25</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="visual">
+        <geometry>
+          <box>
+            <size>0.12 0.025 0.25</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>model://turtlebot3_autorace_2020/traffic_intersection/materials/scripts</uri>
+            <uri>model://turtlebot3_autorace_2020/traffic_intersection/materials/textures</uri>
+            <name>traffic_intersection</name>
+          </script>
+        </material>
+      </visual>
+    </link>    
+  </model>
+</sdf>

turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_left/materials/scripts/traffic_is_left.material

@@ -0,0 +1,14 @@
+material traffic_left
+{
+  technique
+  {
+    pass
+    {
+      texture_unit
+      {
+        texture traffic_left.png
+      }
+    }
+  }
+}
+

turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_left/model.config

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>traffic_left</name>
+  <version>1.0</version>
+  <sdf version='1.6'>model.sdf</sdf>
+
+  <author>
+    <name>Hyunok Lee</name>
+    <email>hyunokhyunok@naver.com</email>
+  </author>
+
+  <description>
+    Model with links of simple shapes and texture applied.
+  </description>
+</model>

turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_left/model.sdf

@@ -0,0 +1,30 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <model name="traffic_left">
+    <static>true</static>
+    <link name="box">
+      <pose>1.08 -0.838 0.125 0 -0 -1.57</pose>
+      <collision name="collision">
+        <geometry>
+          <box>
+            <size>0.12 0.025 0.25</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="visual">
+        <geometry>
+          <box>
+            <size>0.12 0.025 0.25</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>model://turtlebot3_autorace_2020/traffic_left/materials/scripts</uri>
+            <uri>model://turtlebot3_autorace_2020/traffic_left/materials/textures</uri>
+            <name>traffic_left</name>
+          </script>
+        </material>
+      </visual>
+    </link>    
+  </model>
+</sdf>

turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_light/materials/scripts/traffic_light.material

@@ -0,0 +1,41 @@
+material traffic_light_red
+{
+  technique
+  {
+    pass
+    {
+      texture_unit
+      {
+        texture traffic_light_red.png
+      }
+    }
+  }
+}
+
+material traffic_light_green
+{
+  technique
+  {
+    pass
+    {
+      texture_unit
+      {
+        texture traffic_light_green.png
+      }
+    }
+  }
+}
+
+material traffic_light_yellow
+{
+  technique
+  {
+    pass
+    {
+      texture_unit
+      {
+        texture traffic_light_yellow.png
+      }
+    }
+  }
+}

turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_light/model.config

@@ -0,0 +1,13 @@
+<?xml version="1.0" ?>
+<model>
+    <name>traffic_light</name>
+    <version>1.0</version>
+    <sdf version="1.6">model.sdf</sdf>
+    <author>
+        <name>Ashekim</name>
+        <email>ashekim@robotis.com</email>
+        <name>Hyungyu Kim</name>
+        <email>kimhg@robotis.com</email>
+    </author>
+    <description>The traffic light.</description>
+</model>

turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_light/model.sdf

@@ -0,0 +1,32 @@
+<?xml version='1.0'?>
+<sdf version='1.6'>
+  <model name='traffic_light'>
+    <link name='traffic_light'>
+      <pose>0 0 0 0 0 0</pose>
+      <collision name='collision'>
+        <geometry>
+          <box>
+            <size>0.12 0.025 0.25</size>
+          </box>
+        </geometry>   
+      </collision>
+      <visual name='visual'>
+        <geometry>
+          <box>
+            <size>0.12 0.025 0.25</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>model://turtlebot3_autorace_2020/traffic_light/materials/scripts</uri>
+            <uri>model://turtlebot3_autorace_2020/traffic_light/materials/textures</uri>
+            <name>traffic_light_red</name>
+          </script>
+        </material>
+      </visual>
+    </link>
+    <plugin name="traffic_light_plugin" filename="libtraffic_light_plugin.so">
+    </plugin>
+    <static>1</static>
+  </model>
+</sdf>

turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_noentry/materials/scripts/traffic_noentry.material

@@ -0,0 +1,14 @@
+material traffic_noentry
+{
+  technique
+  {
+    pass
+    {
+      texture_unit
+      {
+        texture traffic_noentry.png
+      }
+    }
+  }
+}
+

turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_noentry/model.config

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>traffic_noentry</name>
+  <version>1.0</version>
+  <sdf version='1.6'>model.sdf</sdf>
+
+  <author>
+    <name>Hyunok Lee</name>
+    <email>hyunokhyunok@naver.com</email>
+  </author>
+
+  <description>
+    Model with links of simple shapes and texture applied.
+  </description>
+</model>

turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_noentry/model.sdf

@@ -0,0 +1,30 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <model name="traffic_noentry">
+    <static>true</static>
+    <link name="box">
+      <pose>0 0 0 0 0 0</pose>
+      <collision name="collision">
+        <geometry>
+          <box>
+            <size>0.12 0.025 0.25</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="visual">
+        <geometry>
+          <box>
+            <size>0.12 0.025 0.25</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>model://turtlebot3_autorace_2020/traffic_noentry/materials/scripts</uri>
+            <uri>model://turtlebot3_autorace_2020/traffic_noentry/materials/textures</uri>
+            <name>traffic_noentry</name>
+          </script>
+        </material>
+      </visual>
+    </link>    
+  </model>
+</sdf>

turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_parking/materials/scripts/traffic_parking.material

@@ -0,0 +1,14 @@
+material traffic_parking
+{
+  technique
+  {
+    pass
+    {
+      texture_unit
+      {
+        texture traffic_parking.png
+      }
+    }
+  }
+}
+

turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_parking/model.config

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>traffic_parking</name>
+  <version>1.0</version>
+  <sdf version='1.6'>model.sdf</sdf>
+
+  <author>
+    <name>Hyunok Lee</name>
+    <email>hyunokhyunok@naver.com</email>
+  </author>
+
+  <description>
+    Model with links of simple shapes and texture applied.
+  </description>
+</model>