add tunnel mission

turtlebot3_autorace_tunnel/CMakeLists.txt

@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 3.8)
+project(turtlebot3_autorace_tunnel)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+install(PROGRAMS src
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+install(DIRECTORY map
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

turtlebot3_autorace_tunnel/launch/mission_tunnel.launch.py

@@ -0,0 +1,60 @@
+#!/usr/bin/env python3
+
+################################################################################
+# 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: ChanHyeong Lee
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+    # Get the share directory of turtlebot3_navigation2 package
+    nav2_pkg_share = get_package_share_directory('turtlebot3_navigation2')
+
+    # Map file path
+    map_file = os.path.join(
+            get_package_share_directory('turtlebot3_autorace_tunnel'),
+            'map',
+            'map.yaml')
+
+    # Include the navigation2 launch file with required arguments
+    nav2_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(nav2_pkg_share, 'launch', 'navigation2.launch.py')
+        ),
+        launch_arguments={
+            'use_sim_time': 'True',
+            'map': map_file
+        }.items()
+    )
+
+    # Execute the set_init_pose.py node via ros2 run command
+    mission_tunnel = ExecuteProcess(
+        cmd=['ros2', 'run', 'turtlebot3_autorace_tunnel', 'mission_tunnel.py'],
+        output='screen'
+    )
+
+    return LaunchDescription([
+        nav2_launch,
+        mission_tunnel,
+    ])

turtlebot3_autorace_tunnel/map/map.yaml

@@ -0,0 +1,7 @@
+image: map.pgm
+mode: trinary
+resolution: 0.05
+origin: [-2.11, -4.35, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25

turtlebot3_autorace_tunnel/package.xml

@@ -0,0 +1,25 @@
+<?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>turtlebot3_autorace_tunnel</name>
+  <version>1.0.0</version>
+  <description>ROS2 port of turtlebot3_autorace_tunnel</description>
+  <maintainer email="[email protected]">Pyo</maintainer>
+  <license>Apache 2.0</license>
+
+  <author>ChanHyeong Lee</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+
+  <exec_depend>ament_python</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

turtlebot3_autorace_tunnel/src/mission_tunnel.py

@@ -0,0 +1,130 @@
+#!/usr/bin/env python3
+
+################################################################################
+# 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: ChanHyeong Lee
+
+import math
+
+import rclpy
+from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from rclpy.node import Node
+
+
+class Nav2GoalPublisher(Node):
+    def __init__(self):
+        super().__init__('nav2_goal_publisher')
+        # Create publisher for initial pose (/initialpose topic, PoseWithCovarianceStamped type)
+        self.init_pose_pub = self.create_publisher(PoseWithCovarianceStamped, '/initialpose', 10)
+        # Create publisher for goal pose (/goal_pose topic, PoseStamped type)
+        self.goal_pub = self.create_publisher(PoseStamped, '/goal_pose', 10)
+
+        # Timer to delay the start of publishing by 5 seconds
+        self.start_timer = self.create_timer(1.0, self.start_initial_phase)
+        # Ensure the timer runs only once by cancelling it in the callback
+        self.started = False
+
+        # Goal publishing timer and shutdown timer are not created yet
+        self.init_timer = None
+        self.phase_timer = None
+        self.goal_timer = None
+        self.shutdown_timer = None
+
+    def start_initial_phase(self):
+        # Make sure this callback runs only once
+        if self.started:
+            return
+        self.started = True
+
+        # Cancel the start timer
+        self.start_timer.cancel()
+
+        # Create a timer to publish initial pose every 0.1 seconds (runs for 1 second)
+        self.init_timer = self.create_timer(0.1, self.publish_initial_pose)
+        # Create a timer to switch to goal publishing phase after 1 second
+        self.phase_timer = self.create_timer(1.0, self.start_goal_phase)
+
+    def publish_initial_pose(self):
+        msg = PoseWithCovarianceStamped()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = 'map'
+
+        # Set initial pose
+        msg.pose.pose.position.x = 0.0
+        msg.pose.pose.position.y = 0.0
+        msg.pose.pose.position.z = 0.0
+
+        yaw = math.radians(0)
+        msg.pose.pose.orientation.x = 0.0
+        msg.pose.pose.orientation.y = 0.0
+        msg.pose.pose.orientation.z = math.sin(yaw / 2.0)
+        msg.pose.pose.orientation.w = math.cos(yaw / 2.0)
+
+        msg.pose.covariance = [0.0] * 36
+
+        self.init_pose_pub.publish(msg)
+
+    def start_goal_phase(self):
+        # Cancel the initial pose timer and phase timer
+        if self.init_timer is not None:
+            self.init_timer.cancel()
+        if self.phase_timer is not None:
+            self.phase_timer.cancel()
+
+        # Create a timer to publish goal pose every 0.1 seconds (runs for 1 second)
+        self.goal_timer = self.create_timer(0.1, self.publish_goal)
+        # Create a timer to shutdown the node after 1 second
+        self.shutdown_timer = self.create_timer(2.0, self.shutdown_node)
+
+    def publish_goal(self):
+        goal_msg = PoseStamped()
+        goal_msg.header.stamp = self.get_clock().now().to_msg()
+        goal_msg.header.frame_id = 'map'
+
+        # Set goal pose
+        goal_msg.pose.position.x = 1.4
+        goal_msg.pose.position.y = -1.4
+        goal_msg.pose.position.z = 0.0
+
+        yaw = math.radians(0)
+        goal_msg.pose.orientation.x = 0.0
+        goal_msg.pose.orientation.y = 0.0
+        goal_msg.pose.orientation.z = math.sin(yaw / 2.0)
+        goal_msg.pose.orientation.w = math.cos(yaw / 2.0)
+
+        self.goal_pub.publish(goal_msg)
+
+    def shutdown_node(self):
+        if self.goal_timer is not None:
+            self.goal_timer.cancel()
+        if self.shutdown_timer is not None:
+            self.shutdown_timer.cancel()
+        rclpy.shutdown()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = Nav2GoalPublisher()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+
+
+if __name__ == '__main__':
+    main()