update

2026-05-06 14:23:22 +08:00 · 2026-05-06 14:23:22 +08:00 · ef86933950
parent 80aa31ef4b
commit ef86933950
293 changed files with 359384 additions and 12 deletions
--- a/README.md
+++ b/README.md
@ -1,18 +1,86 @@
-# 开始拉取代码
+# 基于ROS2的无人小车自主探索与建图

+本卡片让机器人在未知办公室环境中自主探索，并生成 2D 栅格地图。当机器人还不知道环境长什么样，无人小车通过本模块知道如何先把环境“走一遍、画出来”。本卡片依赖ROS2 Humble、Nav2、TurtleBot3 Gazebo、SLAM Toolbox 与 explore_lite 完成自主探索与地图保存。
+
+
+## 1. 环境要求
+- Ubuntu 22.04
+- ROS2 Humble
+- Gazebo Sim Harmonic
 ---
-```
-git clone http://git-test.databall.tech:3000/hq/ros2_office_mapping.git

-cd ros2_office_mapping
+### 编译

-# add your files to repo
-git push
+```bash
+cd /workspace
+colcon build --symlink-install
+source /opt/ros/humble/setup.bash
+source install/setup.bash
 ```
---
+
+### 确认 package 能被 ROS2 找到
+
+```bash
+ros2 pkg list | grep explore
 ```
-cd existing_repo
-git remote add origin http://git-test.databall.tech:3000/hq/ros2_office_mapping.git
-git branch -M main
-git push -uf origin main
-```
+预计打印结果：explore_lite
+
+### 注意事项
+
+之后每打开一个新终端，都建议先执行：
+
+```bash
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+export TURTLEBOT3_MODEL=burger
+```
+
+为了方便，也可以写入 `~/.bashrc`：
+
+```bash
+echo 'source /opt/ros/humble/setup.bash' >> ~/.bashrc
+echo 'source /workspace/install/setup.bash' >> ~/.bashrc
+echo 'export TURTLEBOT3_MODEL=burger' >> ~/.bashrc
+source ~/.bashrc
+```
+
+## 2. 启动仿真， Nav2导航栈， SLAM Toolbox异步建图， rviz2
+
+```bash
+./gzsim_run.sh
+
+ros2 launch slam_toolbox online_async_launch.py use_sim_time:=True
+
+ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True headless:=False
+
+ros2 run rviz2 rviz2 -d /opt/ros/humble/share/nav2_bringup/rviz/nav2_default_view.rviz --ros-args -p use_sim_time:=true
+
+```
+
+## 3. 启动explore_lite 自主探索节点（如果机器人在探索时在某一点长时间卡住，可以在rviz中使用Nav2 Goal进行单点导航探索。）
+```bash
+ros2 launch explore_lite explore.launch.py
+```
+
+![图1](assets/picture01.png)
+
+## 4. 保存建图结果
+
+机器人完成环境全区域探索后，新开终端执行地图保存命令，地图将保存至$NAV2_MAP_PATH指定路径
+
+```bash
+export NAV2_MAP_PATH=/workspace/src/turtlebot3_simulations/turtlebot3_gazebo/map
+ros2 run nav2_map_server map_saver_cli -f ${NAV2_MAP_PATH}/office_map
+```
+
+## 5. 模型说明
+该package 基于explore_lite进行 ROS2 适配与二次开发，是未知环境自主建图的核心，实现地图前沿检测、探索目标。m-explore-ros2 package 的src/目录下包含explore.cpp、costmap_client.cpp、frontier_search.cpp，分别实现探索节点主要逻辑、代价地图数据subscribe与同步， frontier_search的核心算法。
+
+* costmap_client.cpp 管理代价地图的数据，核心class 是 explore::Costmap2DClient， 实现了代价地图订阅、格式转换、位姿计算的具体逻辑，为上层提供了线程安全的代价地图数据和机器人实时位姿。这层的作用：给探索模块提供“实时可查询的栅格地图 + 当前机器人位姿”。首先启动时订阅 map 和 map_updates，把 OccupancyGrid 转成 nav2_costmap_2d::Costmap2D（updateFullMap / updatePartialMap）。其次，getRobotPose() 通过 TF 把 robot_base_frame 转到地图坐标系，给后续前沿搜索当起点。
+
+* frontier_search.cpp 实现了前沿检测与优先级排序算法，核心的class 是frontier_exploration::FrontierSearch， 实现了 BFS 前沿检测、前沿属性计算、成本排序的核心逻辑，接收 Costmap2DClient 提供的代价地图，输出按优先级排序的前沿列表。具体来说，searchFrom(position) 从机器人附近 free cell 开始做 BFS，然后把“未知栅格且4邻域有free”的格子判为 frontier cell（isNewFrontierCell），把“未知栅格且4邻域有free”的格子判为 frontier cell（isNewFrontierCell），用 8 邻域把相连 frontier cell 聚成一个 frontier（buildNewFrontier），并计算size（前沿大小）、centroid（几何中心）、min_distance（离机器人最近点距离）。最后按代价进行排序，cost = potential_scale * distance - gain_scale * size
+即“近的更好、大的更好（代价更低）”。
+
+* explore.cpp：探索节点主逻辑，核心的class是explore::Explore，节点继承自 rclcpp::Node，是 ROS2 探索节点的入口类，在主循环中实现了导航调用、状态控制的流程。具体来说，Explore 节点创建 Nav2 action client（navigate_to_pose），定时器周期调用 makePlan()。makePlan() 核心逻辑是：拿机器人位姿->调 search_.searchFrom() 找 frontiers->过滤 blacklist->选代价最低目标并发给 Nav2->若长时间没进展（progress_timeout），把目标加入 blacklist 并重规划。reachedGoal() 根据 action 结果更新。stop/resume 通过 explore/resume topic 控制暂停恢复。可选 return_to_init=true 时，探索结束回初始点。
+
+* launch/explore.launch.py，启动的入口，声明并加载所有配置参数（ planner_frequency、potential_scale、min_frontier_size 等），直接传递给 Explore 节点的构造函数
--- a/assets/picture01.png
+++ b/assets/picture01.png
--- a/gzsim_run.sh
+++ b/gzsim_run.sh
@ -0,0 +1,68 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+
+ROOT="/workspace"
+
+sleep 5
+
+set +u
+source /opt/ros/humble/setup.bash
+source "${ROOT}/install/setup.bash"
+set -u
+
+LOG_DIR="${ROOT}/autorun_logs/autorun_$(date +%Y%m%d_%H%M%S)"
+mkdir -p "$LOG_DIR"
+
+kill_port_listeners() {
+  local port="$1"
+  local pids
+  pids="$(ss -ltnp 2>/dev/null | grep ":${port} " | sed -E 's/.*pid=([0-9]+).*/\1/' | sort -u || true)"
+  if [[ -n "${pids}" ]]; then
+    echo "[run_all] port ${port} already in use, killing: ${pids}"
+    kill ${pids} 2>/dev/null || true
+    sleep 1
+  fi
+}
+
+kill_port_listeners 9002
+
+export TURTLEBOT3_MODEL=burger
+ros2 launch turtlebot3_gazebo turtlebot3_office.launch.py > "${LOG_DIR}/tb3_gzsim.log" 2>&1 &
+GZSIM_PID=$!
+# gz sim -v 4 -s -r empty.sdf > "${LOG_DIR}/empty_gzsim.log" 2>&1 &
+# GZSIM_PID=$!
+
+echo "[autorun] tb3_gzsim pid=${GZSIM_PID}, log=${LOG_DIR}/tb3_gzsim.log"
+
+sleep 8
+
+if ps -p "$GZSIM_PID" > /dev/null; then
+  echo "[autorun] tb3_gzsim is still running"
+else
+  echo "[autorun] tb3_gzsim exited early. Log:"
+  cat "${LOG_DIR}/tb3_gzsim.log"
+fi
+
+echo "[autorun] starting websocket ..."
+
+gz launch "${ROOT}/src/websocket.gzlaunch" > "${LOG_DIR}/websocket.log" 2>&1 &
+WEBSOCKET_PID=$!
+
+echo "[autorun] websocket pid=${WEBSOCKET_PID}, log=${LOG_DIR}/websocket.log"
+
+sleep 10
+
+if ps -p "$WEBSOCKET_PID" > /dev/null; then
+  echo "[autorun] websocket is still running"
+else
+  echo "[autorun] websocket exited early. Log:"
+  cat "${LOG_DIR}/websocket.log"
+  exit 1
+fi
+
+echo "[autorun] all services started."
+echo "[autorun] tb3_gzsim pid=${GZSIM_PID}"
+echo "[autorun] websocket pid=${WEBSOCKET_PID}"
+
+wait
--- a/src/m-explore-ros2/.gitignore
+++ b/src/m-explore-ros2/.gitignore
@ -0,0 +1,3 @@
+*sublime-*
+*.svg
+*.xcf
--- a/src/m-explore-ros2/LICENSE
+++ b/src/m-explore-ros2/LICENSE
@ -0,0 +1,31 @@
+Software License Agreement (BSD License)
+
+Copyright (c) 2015-2016, Carlos Alvarez.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+* Redistributions of source code must retain the above copyright
+  notice, this list of conditions and the following disclaimer.
+* Redistributions in binary form must reproduce the above
+  copyright notice, this list of conditions and the following
+  disclaimer in the documentation and/or other materials provided
+  with the distribution.
+* Neither the name of the Carlos Alvarez nor the names of its
+  contributors may be used to endorse or promote products derived
+  from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
--- a/src/m-explore-ros2/README.md
+++ b/src/m-explore-ros2/README.md
--- a/src/m-explore-ros2/explore/.DS_Store
+++ b/src/m-explore-ros2/explore/.DS_Store
--- a/src/m-explore-ros2/explore/CMakeLists.txt
+++ b/src/m-explore-ros2/explore/CMakeLists.txt
@ -0,0 +1,121 @@
+cmake_minimum_required(VERSION 3.5)
+project(explore_lite)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Set flag depending on distro
+if(NOT DEFINED ENV{ROS_DISTRO})
+  message(FATAL_ERROR "ROS_DISTRO is not defined." )
+endif()
+if("$ENV{ROS_DISTRO}" STREQUAL "eloquent")
+  message(STATUS "Build for ROS2 eloquent")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DELOQUENT")
+elseif("$ENV{ROS_DISTRO}" STREQUAL "dashing")
+  message(STATUS "Build for ROS2 dashing")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DDASHING")
+else()
+  message(STATUS "BuilD for ROS2: " "$ENV{ROS_DISTRO}")
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(nav2_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(map_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
+
+
+set(DEPENDENCIES
+  rclcpp
+  std_msgs
+  sensor_msgs
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+  nav2_msgs
+  nav_msgs
+  map_msgs
+  nav2_costmap_2d
+  visualization_msgs
+)
+
+include_directories(
+    include
+)
+
+install(
+  DIRECTORY include/explore/
+  DESTINATION include/explore/
+)
+
+install(DIRECTORY
+  config
+  DESTINATION share/${PROJECT_NAME}
+)
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+
+add_executable(explore
+  src/costmap_client.cpp
+  src/explore.cpp
+  src/frontier_search.cpp
+)
+
+target_include_directories(explore PUBLIC
+  $<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()
--- a/src/m-explore-ros2/explore/config/params.yaml
+++ b/src/m-explore-ros2/explore/config/params.yaml
@ -0,0 +1,14 @@
+/**:
+  ros__parameters:
+    robot_base_frame: base_link
+    return_to_init: true
+    costmap_topic: map
+    costmap_updates_topic: map_updates
+    visualize: true
+    planner_frequency: 0.2 #0.15
+    progress_timeout: 40.0
+    potential_scale: 1.0 #3.0
+    orientation_scale: 0.0
+    gain_scale: 3.0  #1.0
+    transform_tolerance: 0.3
+    min_frontier_size: 0.5 #0.75
--- a/src/m-explore-ros2/explore/config/params_costmap.yaml
+++ b/src/m-explore-ros2/explore/config/params_costmap.yaml
@ -0,0 +1,13 @@
+explore_node:
+  ros__parameters:
+    robot_base_frame: base_link
+    costmap_topic: /global_costmap/costmap
+    costmap_updates_topic: /global_costmap/costmap_updates
+    visualize: true
+    planner_frequency: 0.2
+    progress_timeout: 30.0
+    potential_scale: 2.0
+    orientation_scale: 0.0
+    gain_scale: 3.0
+    transform_tolerance: 0.3
+    min_frontier_size: 0.3
--- a/src/m-explore-ros2/explore/doc/screenshot.png
+++ b/src/m-explore-ros2/explore/doc/screenshot.png
--- a/src/m-explore-ros2/explore/doc/wiki_doc.txt
+++ b/src/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
--- a/src/m-explore-ros2/explore/include/explore/costmap_client.h
+++ b/src/m-explore-ros2/explore/include/explore/costmap_client.h
@ -0,0 +1,137 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015-2016, Jiri Horner.
+ *  Copyright (c) 2021, Carlos Alvarez, Juan Galvis.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Jiri Horner nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+
+#ifndef COSTMAP_CLIENT_
+#define COSTMAP_CLIENT_
+
+#include <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
--- a/src/m-explore-ros2/explore/include/explore/costmap_tools.h
+++ b/src/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
--- a/src/m-explore-ros2/explore/include/explore/explore.h
+++ b/src/m-explore-ros2/explore/include/explore/explore.h
@ -0,0 +1,153 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Robert Bosch LLC.
+ *  Copyright (c) 2015-2016, Jiri Horner.
+ *  Copyright (c) 2021, Carlos Alvarez, Juan Galvis.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Jiri Horner nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+#ifndef NAV_EXPLORE_H_
+#define NAV_EXPLORE_H_
+
+#include <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
--- a/src/m-explore-ros2/explore/include/explore/frontier_search.h
+++ b/src/m-explore-ros2/explore/include/explore/frontier_search.h
@ -0,0 +1,87 @@
+#ifndef FRONTIER_SEARCH_H_
+#define FRONTIER_SEARCH_H_
+
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+
+namespace frontier_exploration
+{
+/**
+ * @brief Represents a frontier
+ *
+ */
+struct Frontier {
+  std::uint32_t size;
+  double min_distance;
+  double cost;
+  geometry_msgs::msg::Point initial;
+  geometry_msgs::msg::Point centroid;
+  geometry_msgs::msg::Point middle;
+  std::vector<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
--- a/src/m-explore-ros2/explore/launch/explore.launch.py
+++ b/src/m-explore-ros2/explore/launch/explore.launch.py
@ -0,0 +1,46 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch_ros.actions import Node
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    ld = LaunchDescription()
+    config = os.path.join(
+        get_package_share_directory("explore_lite"), "config", "params.yaml"
+    )
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    namespace = LaunchConfiguration("namespace")
+
+    declare_use_sim_time_argument = DeclareLaunchArgument(
+        "use_sim_time", default_value="true", description="Use simulation/Gazebo clock"
+    )
+    declare_namespace_argument = DeclareLaunchArgument(
+        "namespace",
+        default_value="",
+        description="Namespace for the explore node",
+    )
+
+    # Map fully qualified names to relative ones so the node's namespace can be prepended.
+    # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
+    # https://github.com/ros/geometry2/issues/32
+    # https://github.com/ros/robot_state_publisher/pull/30
+    remappings = [("/tf", "tf"), ("/tf_static", "tf_static")]
+
+    node = Node(
+        package="explore_lite",
+        name="explore_node",
+        namespace=namespace,
+        executable="explore",
+        parameters=[config, {"use_sim_time": use_sim_time}],
+        output="screen",
+        remappings=remappings,
+    )
+    ld.add_action(declare_use_sim_time_argument)
+    ld.add_action(declare_namespace_argument)
+    ld.add_action(node)
+    return ld
--- a/src/m-explore-ros2/explore/package.xml
+++ b/src/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>
--- a/src/m-explore-ros2/explore/src/costmap_client.cpp
+++ b/src/m-explore-ros2/explore/src/costmap_client.cpp
@ -0,0 +1,260 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015-2016, Jiri Horner.
+ *  Copyright (c) 2021, Carlos Alvarez, Juan Galvis.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Jiri Horner nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+
+#include <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
--- a/src/m-explore-ros2/explore/src/explore.cpp
+++ b/src/m-explore-ros2/explore/src/explore.cpp
@ -0,0 +1,477 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Robert Bosch LLC.
+ *  Copyright (c) 2015-2016, Jiri Horner.
+ *  Copyright (c) 2021, Carlos Alvarez, Juan Galvis.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Jiri Horner nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+
+#include <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;
+}
--- a/src/m-explore-ros2/explore/src/frontier_search.cpp
+++ b/src/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
--- a/src/m-explore-ros2/explore/test/test_explore.cpp
+++ b/src/m-explore-ros2/explore/test/test_explore.cpp
@ -0,0 +1,77 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2022, Carlos Alvarez.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Carlos Alvarez nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+
+#include <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();
+}
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/CHANGELOG.rst
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/CHANGELOG.rst
@ -0,0 +1,187 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package turtlebot3_gazebo
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+2.3.8 (2025-07-10)
+------------------
+* None
+
+2.3.6 (2025-06-19)
+------------------
+* None
+
+2.3.4 (2025-05-28)
+------------------
+* None
+
+2.3.0 (2025-02-17)
+------------------
+* Added multi-robot launch functionality
+* Updated robot mesh in Gazebo and RViz
+* Added launch file for TurtleBot3 Autorace 2020
+* Added plugins to the models of Autorace 2020
+* Contributors: Hyungyu Kim
+
+2.2.6 (2022-05-26)
+------------------
+* ROS 2 Humble Hawksbill supported
+* Contributors: Will Son
+
+2.2.5 (2021-08-25)
+------------------
+* Release for ROS 2 Rolling
+* Contributors: Will Son
+
+2.2.4 (2021-06-14)
+------------------
+* Release for ROS 2 Galactic
+* Separate world and robot models(#162)
+* Clean up unncessary files
+* Use turtlebot3_common mesh modeling
+* Independent turtlebot3_simulations package
+* Contributors: Joep Tool, Will Son
+
+2.2.3 (2021-04-12)
+------------------
+* Update required keyword arguments
+* Clear up exec_depend
+* Fix Waffle Pi wheel inertia
+* Contributors: ruffsl, Will Son
+
+2.2.2 (2021-02-24)
+------------------
+* Remove shared objects built in older version
+* Contributors: Will Son
+
+2.2.1 (2021-01-13)
+------------------
+* Eloquent Elusor EOL
+* Add missing imu joint in sdf
+* Append Gazebo model path
+* Portable fix, launch description revise
+* Ament lint applied
+* Contributors: minwoominwoominwoo7, Rayman, seanyen, ashe kim, Will Son
+
+2.2.0 (2020-06-29)
+------------------
+* TurtleBot3 Drive node implementation
+* Additional Gazebo maps added
+* argument tags in the sdf file replaced with remapping tags
+* Low polygon 3D modeling applied for simulation
+* Contributors: Ryan Shim, Mikael Arguedas, Will Son
+
+2.1.0 (2019-09-10)
+------------------
+* Added turtlebot3_house and related world, model files
+* Contributors: Ryan Shim
+
+2.0.1 (2019-09-05)
+------------------
+* Modified dependency packages
+* Modified launch directory
+* Added a launch file for robot state publisher
+* Contributors: Darby Lim, Pyo
+
+2.0.0 (2019-08-20)
+------------------
+* Supported ROS 2 Dashing Diademata
+* Updated the CHANGELOG and version to release binary packages
+* Contributors: Darby Lim, Pyo
+
+1.3.0 (2020-06-29)
+------------------
+* Turtlebot3 Autorace 2020 implemented
+* Remove the plugin_path from gazebo_ros export
+* Remove *nix path separator
+* Contributors: Ashe Kim, Ben Wolsieffer, Sean Yen
+
+1.2.0 (2019-01-22)
+------------------
+* moved <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
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/CMakeLists.txt
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/CMakeLists.txt
@ -0,0 +1,139 @@
+################################################################################
+# Set minimum required version of cmake, project name and compile options
+################################################################################
+cmake_minimum_required(VERSION 3.5)
+project(turtlebot3_gazebo)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+if(MSVC)
+  add_compile_definitions(_USE_MATH_DEFINES)
+endif()
+
+################################################################################
+# Find ament packages and libraries for ament and system dependencies
+# NOTE: Migrating from Gazebo Classic to Gazebo Sim (gz-8 / Harmonic)
+################################################################################
+find_package(ament_cmake REQUIRED)
+
+# ROS 2 core packages
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+
+# Gazebo Sim (Harmonic) libraries
+find_package(gz-sim8 REQUIRED)
+find_package(gz-msgs10 REQUIRED)
+find_package(gz-transport13 REQUIRED)
+find_package(gz-physics7 REQUIRED)
+find_package(gz-math7 REQUIRED)
+find_package(gz-common5 REQUIRED)
+find_package(gz-plugin2 REQUIRED)
+find_package(sdformat14 REQUIRED)
+
+# Optional: ros_gz bridge packages (for launch integration)
+find_package(ros_gz_bridge QUIET)
+find_package(ros_gz_sim QUIET)
+
+################################################################################
+# Build
+################################################################################
+
+include_directories(
+  include
+  ${gz-sim8_INCLUDE_DIRS}
+  ${gz-msgs10_INCLUDE_DIRS}
+  ${gz-transport13_INCLUDE_DIRS}
+  ${gz-physics7_INCLUDE_DIRS}
+  ${gz-math7_INCLUDE_DIRS}
+  ${gz-common5_INCLUDE_DIRS}
+  ${gz-plugin2_INCLUDE_DIRS}
+  ${sdformat14_INCLUDE_DIRS}
+)
+
+set(dependencies
+  "geometry_msgs"
+  "nav_msgs"
+  "rclcpp"
+  "sensor_msgs"
+  "tf2"
+)
+
+# --- Executable: turtlebot3_drive (ROS 2 navigation node, no gazebo deps) ---
+set(EXEC_NAME "turtlebot3_drive")
+add_executable(${EXEC_NAME} src/turtlebot3_drive.cpp)
+ament_target_dependencies(${EXEC_NAME} ${dependencies})
+
+# --- Shared Libraries: Custom Gazebo Sim System Plugins ---
+# NOTE: These plugins originally used the Gazebo Classic API (gazebo::ModelPlugin)
+#       with headers like <gazebo/gazebo.hh>. They must be rewritten to use the
+#       gz-sim System API (ISystemPreUpdate, EntityComponentManager, etc.)
+#       before they can compile. Default OFF until porting is done.
+option(BUILD_GAZEBO_PLUGINS "Build custom Gazebo Sim system plugins" OFF)
+
+if(BUILD_GAZEBO_PLUGINS)
+  set(GZ_PLUGIN_LIBRARIES
+    gz-sim8::gz-sim8
+    gz-msgs10::gz-msgs10
+    gz-transport13::gz-transport13
+    gz-physics7::gz-physics7
+    gz-math7::gz-math7
+    gz-common5::gz-common5
+    gz-plugin2::gz-plugin2
+    sdformat14::sdformat14
+  )
+
+  add_library(traffic_light_plugin SHARED src/traffic_light_plugin.cpp)
+  target_link_libraries(traffic_light_plugin ${GZ_PLUGIN_LIBRARIES})
+
+  add_library(traffic_bar_plugin SHARED src/traffic_bar_plugin.cpp)
+  target_link_libraries(traffic_bar_plugin ${GZ_PLUGIN_LIBRARIES})
+
+  add_library(obstacle1 SHARED src/obstacle1.cpp)
+  target_link_libraries(obstacle1 ${GZ_PLUGIN_LIBRARIES})
+
+  add_library(obstacle2 SHARED src/obstacle2.cpp)
+  target_link_libraries(obstacle2 ${GZ_PLUGIN_LIBRARIES})
+
+  add_library(obstacles SHARED src/obstacles.cpp)
+  target_link_libraries(obstacles ${GZ_PLUGIN_LIBRARIES})
+endif()
+
+################################################################################
+# Install
+################################################################################
+install(TARGETS ${EXEC_NAME}
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY launch config models rviz urdf worlds scripts gui
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+# Install Python scripts with correct permissions
+install(PROGRAMS scripts/scan_frame_fix.py scripts/camera_topic_remap.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+################################################################################
+# Export & Macro for ament package
+################################################################################
+ament_export_include_directories(include)
+ament_export_dependencies(geometry_msgs)
+ament_export_dependencies(nav_msgs)
+ament_export_dependencies(rclcpp)
+ament_export_dependencies(sensor_msgs)
+ament_export_dependencies(tf2)
+ament_package()
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/config/turtlebot3_gz_bridge.yaml
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/config/turtlebot3_gz_bridge.yaml
@ -0,0 +1,42 @@
+# TurtleBot3 Burger - Gz Sim to ROS 2 Bridge Configuration
+# For use with ros_gz_bridge bridge_node
+#
+# Key migration notes (Gazebo Classic -> Gazebo Sim):
+#   1. gz-sim-diff-drive publishes Odometry but NOT TF (unlike Classic).
+#      TF is bridged from /world/<world>/pose/info (gz.msgs.Pose_V -> TFMessage).
+#   2. Sensor frame_ids use Gazebo scoped names (e.g. burger/base_scan/...).
+#      Overridden below via 'frame_id' field to match URDF/robot_state_publisher tree.
+
+# === TF: bridge Gazebo model tf to ROS2 /tf ===
+# /model/burger/tf publishes gz.msgs.Pose_V (NOT Pose!)
+# ros_gz_bridge converts Pose_V -> TFMessage correctly.
+- ros_topic_name: /tf
+  ros_type_name: tf2_msgs/msg/TFMessage
+  gz_topic_name: /model/burger/tf
+  gz_type_name: gz.msgs.Pose_V
+  direction: GZ_TO_ROS
+
+# === Sensor bridges ===
+- ros_topic_name: /scan_raw
+  ros_type_name: sensor_msgs/msg/LaserScan
+  gz_topic_name: /world/default/model/burger/link/base_scan/sensor/hls_lfcd_lds/scan
+  gz_type_name: gz.msgs.LaserScan
+  direction: GZ_TO_ROS
+
+- ros_topic_name: /clock
+  ros_type_name: rosgraph_msgs/msg/Clock
+  gz_topic_name: /clock
+  gz_type_name: gz.msgs.Clock
+  direction: GZ_TO_ROS
+
+- ros_topic_name: /cmd_vel
+  ros_type_name: geometry_msgs/msg/Twist
+  gz_topic_name: /model/burger/cmd_vel
+  gz_type_name: gz.msgs.Twist
+  direction: ROS_TO_GZ
+
+- ros_topic_name: /odom
+  ros_type_name: nav_msgs/msg/Odometry
+  gz_topic_name: /model/burger/odometry
+  gz_type_name: gz.msgs.Odometry
+  direction: GZ_TO_ROS
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/gui/office_gui.config
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/gui/office_gui.config
@ -0,0 +1,270 @@
+<?xml version="1.0"?>
+
+<!-- Quick start dialog -->
+<dialog name="quick_start" show_again="true"/>
+
+<!-- Window -->
+<window>
+  <width>1400</width>
+  <height>900</height>
+  <style
+    material_theme="Light"
+    material_primary="DeepOrange"
+    material_accent="LightBlue"
+    toolbar_color_light="#f3f3f3"
+    toolbar_text_color_light="#111111"
+    toolbar_color_dark="#414141"
+    toolbar_text_dark="#f3f3f3"
+    plugin_toolbar_color_light="#bbdefb"
+    plugin_toolbar_text_color_light="#111111"
+    plugin_toolbar_color_dark="#607d8b"
+    plugin_toolbar_text_color_dark="#eeeeee"
+  />
+  <menus>
+    <drawer default="false">
+    </drawer>
+  </menus>
+  <dialog_on_exit>true</dialog_on_exit>
+</window>
+
+<!-- GUI plugins -->
+
+<!-- 3D scene -->
+<plugin filename="MinimalScene" name="3D View">
+  <gz-gui>
+    <title>3D View</title>
+    <property type="bool" key="showTitleBar">false</property>
+    <property type="string" key="state">docked</property>
+  </gz-gui>
+
+  <engine>ogre2</engine>
+  <scene>scene</scene>
+  <ambient_light>0.4 0.4 0.4</ambient_light>
+  <background_color>0.8 0.8 0.8</background_color>
+  <camera_pose>0 -5 12 0 1.2 1.57</camera_pose>
+</plugin>
+
+<!-- Plugins that add functionality to the scene -->
+<plugin filename="EntityContextMenuPlugin" name="Entity context menu">
+  <gz-gui>
+    <property key="state" type="string">floating</property>
+    <property key="width" type="double">5</property>
+    <property key="height" type="double">5</property>
+    <property key="showTitleBar" type="bool">false</property>
+  </gz-gui>
+</plugin>
+<plugin filename="GzSceneManager" name="Scene Manager">
+  <gz-gui>
+    <property key="resizable" type="bool">false</property>
+    <property key="width" type="double">5</property>
+    <property key="height" type="double">5</property>
+    <property key="state" type="string">floating</property>
+    <property key="showTitleBar" type="bool">false</property>
+  </gz-gui>
+</plugin>
+<plugin filename="InteractiveViewControl" name="Interactive view control">
+  <gz-gui>
+    <property key="resizable" type="bool">false</property>
+    <property key="width" type="double">5</property>
+    <property key="height" type="double">5</property>
+    <property key="state" type="string">floating</property>
+    <property key="showTitleBar" type="bool">false</property>
+  </gz-gui>
+</plugin>
+<plugin filename="CameraTracking" name="Camera Tracking">
+  <gz-gui>
+    <property key="resizable" type="bool">false</property>
+    <property key="width" type="double">5</property>
+    <property key="height" type="double">5</property>
+    <property key="state" type="string">floating</property>
+    <property key="showTitleBar" type="bool">false</property>
+  </gz-gui>
+</plugin>
+<plugin filename="MarkerManager" name="Marker manager">
+  <gz-gui>
+    <property key="resizable" type="bool">false</property>
+    <property key="width" type="double">5</property>
+    <property key="height" type="double">5</property>
+    <property key="state" type="string">floating</property>
+    <property key="showTitleBar" type="bool">false</property>
+  </gz-gui>
+</plugin>
+<plugin filename="SelectEntities" name="Select Entities">
+  <gz-gui>
+    <property key="resizable" type="bool">false</property>
+    <property key="width" type="double">5</property>
+    <property key="height" type="double">5</property>
+    <property key="state" type="string">floating</property>
+    <property key="showTitleBar" type="bool">false</property>
+  </gz-gui>
+</plugin>
+
+<plugin filename="Spawn" name="Spawn Entities">
+  <gz-gui>
+    <property key="resizable" type="bool">false</property>
+    <property key="width" type="double">5</property>
+    <property key="height" type="double">5</property>
+    <property key="state" type="string">floating</property>
+    <property key="showTitleBar" type="bool">false</property>
+  </gz-gui>
+</plugin>
+
+<plugin filename="VisualizationCapabilities" name="Visualization Capabilities">
+  <gz-gui>
+    <property key="resizable" type="bool">false</property>
+    <property key="width" type="double">5</property>
+    <property key="height" type="double">5</property>
+    <property key="state" type="string">floating</property>
+    <property key="showTitleBar" type="bool">false</property>
+  </gz-gui>
+</plugin>
+
+<!-- World control -->
+<plugin filename="WorldControl" name="World control">
+  <gz-gui>
+    <title>World control</title>
+    <property type="bool" key="showTitleBar">false</property>
+    <property type="bool" key="resizable">false</property>
+    <property type="double" key="height">72</property>
+    <property type="double" key="z">1</property>
+
+    <property type="string" key="state">floating</property>
+    <anchors target="3D View">
+      <line own="left" target="left"/>
+      <line own="bottom" target="bottom"/>
+    </anchors>
+  </gz-gui>
+
+  <play_pause>true</play_pause>
+  <step>true</step>
+  <start_paused>true</start_paused>
+  <use_event>true</use_event>
+
+</plugin>
+
+<!-- World statistics -->
+<plugin filename="WorldStats" name="World stats">
+  <gz-gui>
+    <title>World stats</title>
+    <property type="bool" key="showTitleBar">false</property>
+    <property type="bool" key="resizable">false</property>
+    <property type="double" key="height">110</property>
+    <property type="double" key="width">290</property>
+    <property type="double" key="z">1</property>
+
+    <property type="string" key="state">floating</property>
+    <anchors target="3D View">
+      <line own="right" target="right"/>
+      <line own="bottom" target="bottom"/>
+    </anchors>
+  </gz-gui>
+
+  <sim_time>true</sim_time>
+  <real_time>true</real_time>
+  <real_time_factor>true</real_time_factor>
+  <iterations>true</iterations>
+</plugin>
+
+<!-- Insert simple shapes -->
+<plugin filename="Shapes" name="Shapes">
+  <gz-gui>
+    <property key="resizable" type="bool">false</property>
+    <property key="x" type="double">0</property>
+    <property key="y" type="double">0</property>
+    <property key="width" type="double">300</property>
+    <property key="height" type="double">50</property>
+    <property key="state" type="string">floating</property>
+    <property key="showTitleBar" type="bool">false</property>
+    <property key="cardBackground" type="string">#666666</property>
+  </gz-gui>
+</plugin>
+
+<!-- Insert lights -->
+<plugin filename="Lights" name="Lights">
+  <gz-gui>
+    <property key="resizable" type="bool">false</property>
+    <property key="x" type="double">300</property>
+    <property key="y" type="double">0</property>
+    <property key="width" type="double">150</property>
+    <property key="height" type="double">50</property>
+    <property key="state" type="string">floating</property>
+    <property key="showTitleBar" type="bool">false</property>
+    <property key="cardBackground" type="string">#666666</property>
+  </gz-gui>
+</plugin>
+
+<!-- Translate / rotate -->
+<plugin filename="TransformControl" name="Transform control">
+  <gz-gui>
+    <property key="resizable" type="bool">false</property>
+    <property key="x" type="double">0</property>
+    <property key="y" type="double">50</property>
+    <property key="width" type="double">250</property>
+    <property key="height" type="double">50</property>
+    <property key="state" type="string">floating</property>
+    <property key="showTitleBar" type="bool">false</property>
+    <property key="cardBackground" type="string">#777777</property>
+  </gz-gui>
+</plugin>
+
+<!-- Screenshot -->
+<plugin filename="Screenshot" name="Screenshot">
+  <gz-gui>
+    <property key="resizable" type="bool">false</property>
+    <property key="x" type="double">250</property>
+    <property key="y" type="double">50</property>
+    <property key="width" type="double">50</property>
+    <property key="height" type="double">50</property>
+    <property key="state" type="string">floating</property>
+    <property key="showTitleBar" type="bool">false</property>
+    <property key="cardBackground" type="string">#777777</property>
+  </gz-gui>
+</plugin>
+
+<!-- Copy/Paste -->
+<plugin filename="CopyPaste" name="CopyPaste">
+  <gz-gui>
+    <property key="resizable" type="bool">false</property>
+    <property key="x" type="double">300</property>
+    <property key="y" type="double">50</property>
+    <property key="width" type="double">100</property>
+    <property key="height" type="double">50</property>
+    <property key="state" type="string">floating</property>
+    <property key="showTitleBar" type="bool">false</property>
+    <property key="cardBackground" type="string">#777777</property>
+  </gz-gui>
+</plugin>
+
+<!-- Inspector -->
+<plugin filename="ComponentInspector" name="Component inspector">
+  <gz-gui>
+    <property type="bool" key="showTitleBar">false</property>
+    <property type="string" key="state">docked</property>
+  </gz-gui>
+</plugin>
+
+<!-- Entity tree -->
+<plugin filename="EntityTree" name="Entity tree">
+  <gz-gui>
+    <property type="bool" key="showTitleBar">false</property>
+    <property type="string" key="state">docked</property>
+  </gz-gui>
+</plugin>
+
+<!-- Image Display - show camera images from GzSim sensors -->
+<plugin filename="ImageDisplay" name="Image Display">
+  <gz-gui>
+    <title>Image Display</title>
+    <property type="bool" key="showTitleBar">true</property>
+    <property type="string" key="state">docked</property>
+  </gz-gui>
+</plugin>
+
+<!-- Visualize Lidar - show lidar point cloud visualization -->
+<plugin filename="VisualizeLidar" name="Visualize Lidar">
+  <gz-gui>
+    <title>Visualize Lidar</title>
+    <property type="bool" key="showTitleBar">true</property>
+    <property type="string" key="state">docked</property>
+  </gz-gui>
+</plugin>
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/include/turtlebot3_gazebo/obstacle1.hpp
+++ b/src/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_
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/include/turtlebot3_gazebo/obstacle2.hpp
+++ b/src/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_
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/include/turtlebot3_gazebo/obstacles.hpp
+++ b/src/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_
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/include/turtlebot3_gazebo/traffic_bar_plugin.hpp
+++ b/src/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_
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/include/turtlebot3_gazebo/traffic_light_plugin.hpp
+++ b/src/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_
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/include/turtlebot3_gazebo/turtlebot3_drive.hpp
+++ b/src/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_
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/launch/empty_world.launch.py
+++ b/src/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
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/launch/multi_robot.launch.py
+++ b/src/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
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/launch/multi_spawn_turtlebot3.launch.py
+++ b/src/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
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/launch/robot_state_publisher.launch.py
+++ b/src/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, "/'"])
+            }],
+        ),
+    ])
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/launch/spawn_turtlebot3.launch.py
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/launch/spawn_turtlebot3.launch.py
@ -0,0 +1,87 @@
+# 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.
+# Migrated from Gazebo Classic to Gazebo Sim (gz-sim / Harmonic)
+
+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():
+    # Pre-resolve package paths (portable across environments)
+    turtlebot3_gazebo_share = get_package_share_directory('turtlebot3_gazebo')
+
+    # Get the SDF file
+    TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL']
+    model_folder = 'turtlebot3_' + TURTLEBOT3_MODEL
+    sdf_path = os.path.join(
+        turtlebot3_gazebo_share,
+        'models',
+        model_folder,
+        'model_gz.sdf'
+    )
+
+    # Bridge configuration YAML (avoids --ros-args -p @ parsing issue)
+    bridge_config_path = os.path.join(
+        turtlebot3_gazebo_share, 'config', 'turtlebot3_gz_bridge.yaml'
+    )
+
+    # 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_gz_spawner_cmd = Node(
+        package='ros_gz_sim',
+        executable='create',
+        arguments=[
+            '-name', TURTLEBOT3_MODEL,
+            '-file', sdf_path,
+            '-x', x_pose,
+            '-y', y_pose,
+            '-z', '0.03'
+        ],
+        output='screen',
+    )
+
+    start_bridge_cmd = Node(
+        package='ros_gz_bridge',
+        executable='bridge_node',
+        parameters=[{
+            'config_file': bridge_config_path,
+            'use_sim_time': True,
+        }],
+        output='screen'
+    )
+
+    ld = LaunchDescription()
+
+    ld.add_action(declare_x_position_cmd)
+    ld.add_action(declare_y_position_cmd)
+    ld.add_action(start_gz_spawner_cmd)
+    ld.add_action(start_bridge_cmd)
+
+    return ld
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_autorace_2020.launch.py
+++ b/src/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
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_dqn_stage1.launch.py
+++ b/src/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
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_dqn_stage2.launch.py
+++ b/src/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
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_dqn_stage3.launch.py
+++ b/src/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
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_dqn_stage4.launch.py
+++ b/src/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
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_house.launch.py
+++ b/src/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
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_office.launch.py
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_office.launch.py
@ -0,0 +1,100 @@
+#!/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
+# Migrated from Gazebo Classic to Gazebo Sim (gz-sim / Harmonic)
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch')
+
+    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')
+
+    pkg_share_dir = get_package_share_directory('turtlebot3_gazebo')
+    world = os.path.join(pkg_share_dir, 'worlds', 'office_gz_dartsim.sdf')
+    models_path = os.path.join(pkg_share_dir, 'models')
+    office_models_path = os.path.join(models_path, 'turtlebot3_office')
+
+    # Headless mode: no GUI, no RVIZ
+    # gui_config = os.path.join(pkg_share_dir, 'gui', 'office_gui.config')
+    # gz_args = f'-r {world} --gui-config {gui_config}'
+    gz_args = f'-r -s {world}'
+    # gz_args = f'-r {world}'
+
+    gz_sim_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(
+                get_package_share_directory('ros_gz_sim'),
+                'launch',
+                'gz_sim.launch.py'
+            )
+        ),
+        launch_arguments={'gz_args': gz_args}.items()
+    )
+
+    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()
+    )
+
+    # Fix LaserScan frame_id:
+    #   bridge publishes on /scan_raw (frame_id = 'burger/base_scan/hls_lfcd_lds')
+    #   this node republishes on /scan with corrected frame_id = 'base_scan'
+    scan_frame_fix_cmd = Node(
+        package='turtlebot3_gazebo',
+        executable='scan_frame_fix.py',
+        output='screen',
+        parameters=[{'use_sim_time': True}],
+    )
+
+    ld = LaunchDescription()
+
+    # Set GZ_SIM_RESOURCE_PATH so gz sim can resolve model:// URIs
+    ld.add_action(SetEnvironmentVariable(
+        name='GZ_SIM_RESOURCE_PATH',
+        value=office_models_path + os.pathsep + models_path
+    ))
+
+    # Add the commands to the launch description
+    ld.add_action(gz_sim_cmd)
+    ld.add_action(robot_state_publisher_cmd)
+    ld.add_action(spawn_turtlebot_cmd)
+    ld.add_action(scan_frame_fix_cmd)
+
+    return ld
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_world.launch.py
+++ b/src/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
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/map/office_map_old.pgm
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/map/office_map_old.pgm
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/map/office_map_old.yaml
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/map/office_map_old.yaml
@ -0,0 +1,7 @@
+image: office_map.pgm
+mode: trinary
+resolution: 0.05
+origin: [-5.71, -2.68, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/checker/materials/checker.png
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/checker/materials/checker.png
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/checker/materials/scripts/checker.material
+++ b/src/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
+      }
+    }
+  }
+}
+
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/checker/materials/textures/checker.png
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/checker/materials/textures/checker.png
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/checker/model.config
+++ b/src/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>
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/checker/model.sdf
+++ b/src/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>
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/course/materials/scripts/course.material
+++ b/src/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
+      }
+    }
+  }
+}
+
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/course/materials/textures/course.png
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/course/materials/textures/course.png
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/course/model.config
+++ b/src/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>
+
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/course/model.sdf
+++ b/src/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>
+
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_bar/materials/scripts/traffic_bar.material
+++ b/src/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
+      }
+    }
+  }
+}
+
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_bar/materials/textures/traffic_bar.png
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_bar/materials/textures/traffic_bar.png
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_bar/model.config
+++ b/src/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>
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_bar/model.sdf
+++ b/src/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>
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_construction/materials/scripts/traffic_construction.material
+++ b/src/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
+      }
+    }
+  }
+}
+
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_construction/materials/textures/traffic_construction.png
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_construction/materials/textures/traffic_construction.png
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_construction/model.config
+++ b/src/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>
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_construction/model.sdf
+++ b/src/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>
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_intersection/materials/scripts/traffic_intersection.material
+++ b/src/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
+      }
+    }
+  }
+}
+
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_intersection/materials/textures/traffic_intersection.png
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_intersection/materials/textures/traffic_intersection.png
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_intersection/model.config
+++ b/src/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>
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_intersection/model.sdf
+++ b/src/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>
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_left/materials/scripts/traffic_is_left.material
+++ b/src/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
+      }
+    }
+  }
+}
+
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_left/materials/textures/traffic_left.png
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_left/materials/textures/traffic_left.png
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_left/model.config
+++ b/src/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>
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_left/model.sdf
+++ b/src/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>
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_light/materials/scripts/traffic_light.material
+++ b/src/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
+      }
+    }
+  }
+}
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_light/materials/textures/traffic_light.png
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_light/materials/textures/traffic_light.png
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_light/materials/textures/traffic_light_green.png
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_light/materials/textures/traffic_light_green.png
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_light/materials/textures/traffic_light_red.png
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_light/materials/textures/traffic_light_red.png
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_light/materials/textures/traffic_light_yellow.png
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_light/materials/textures/traffic_light_yellow.png
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_light/model.config
+++ b/src/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>
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_light/model.sdf
+++ b/src/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>
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_noentry/materials/scripts/traffic_noentry.material
+++ b/src/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
+      }
+    }
+  }
+}
+
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_noentry/materials/textures/traffic_noentry.png
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_noentry/materials/textures/traffic_noentry.png
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_noentry/model.config
+++ b/src/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>
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_noentry/model.sdf
+++ b/src/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>
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_parking/materials/scripts/traffic_parking.material
+++ b/src/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
+      }
+    }
+  }
+}
+
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_parking/materials/textures/traffic_parking.png
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_parking/materials/textures/traffic_parking.png
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_parking/model.config
+++ b/src/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>
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_parking/model.sdf
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_parking/model.sdf
@ -0,0 +1,30 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <model name="traffic_parking">
+    <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_parking/materials/scripts</uri>
+            <uri>model://turtlebot3_autorace_2020/traffic_parking/materials/textures</uri>
+            <name>traffic_parking</name>
+          </script>
+        </material>
+      </visual>
+    </link>    
+  </model>
+</sdf>
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_pl_left/materials/scripts/traffic_pl_left.material
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_pl_left/materials/scripts/traffic_pl_left.material
@ -0,0 +1,14 @@
+material traffic_pl_left
+{
+  technique
+  {
+    pass
+    {
+      texture_unit
+      {
+        texture traffic_pl_left.png
+      }
+    }
+  }
+}
+
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_pl_left/materials/textures/traffic_pl_left.png
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_pl_left/materials/textures/traffic_pl_left.png
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_pl_left/model.config
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_pl_left/model.config
@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>traffic_pl_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>
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_pl_left/model.sdf
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_pl_left/model.sdf
@ -0,0 +1,30 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <model name="traffic_pl_left">
+    <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_pl_left/materials/scripts</uri>
+            <uri>model://turtlebot3_autorace_2020/traffic_pl_left/materials/textures</uri>
+            <name>traffic_pl_left</name>
+          </script>
+        </material>
+      </visual>
+    </link>    
+  </model>
+</sdf>
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_right/materials/scripts/traffic_right.material
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_right/materials/scripts/traffic_right.material
@ -0,0 +1,14 @@
+material traffic_right
+{
+  technique
+  {
+    pass
+    {
+      texture_unit
+      {
+        texture traffic_right.png
+      }
+    }
+  }
+}
+
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_right/materials/textures/traffic_right.png
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_right/materials/textures/traffic_right.png
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_right/model.config
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_right/model.config
@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>traffic_right</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>
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_right/model.sdf
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_right/model.sdf
@ -0,0 +1,30 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <model name="traffic_right">
+    <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_right/materials/scripts</uri>
+            <uri>model://turtlebot3_autorace_2020/traffic_right/materials/textures</uri>
+            <name>traffic_right</name>
+          </script>
+        </material>
+      </visual>
+    </link>    
+  </model>
+</sdf>
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_stop/materials/scripts/traffic_stop.material
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_stop/materials/scripts/traffic_stop.material
@ -0,0 +1,14 @@
+material traffic_stop
+{
+  technique
+  {
+    pass
+    {
+      texture_unit
+      {
+        texture traffic_stop.png
+      }
+    }
+  }
+}
+
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_stop/materials/textures/traffic_stop.png
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_stop/materials/textures/traffic_stop.png
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_stop/model.config
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_stop/model.config
@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>traffic_stop</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>
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_stop/model.sdf
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_stop/model.sdf
@ -0,0 +1,30 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <model name="traffic_stop">
+    <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_stop/materials/scripts</uri>
+            <uri>model://turtlebot3_autorace_2020/traffic_stop/materials/textures</uri>
+            <name>traffic_stop</name>
+          </script>
+        </material>
+      </visual>
+    </link>    
+  </model>
+</sdf>
--- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_tunnel/materials/scripts/tunnel.material
+++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_autorace_2020/traffic_tunnel/materials/scripts/tunnel.material
@ -0,0 +1,14 @@
+material traffic_tunnel
+{
+  technique
+  {
+    pass
+    {
+      texture_unit
+      {
+        texture tunnel.png
+      }
+    }
+  }
+}
+
--- a/Show More
+++ b/Show More