diff --git a/turtlebot3_autorace_tunnel/CHANGELOG.rst b/turtlebot3_autorace_tunnel/CHANGELOG.rst new file mode 100644 index 0000000..e69de29 diff --git a/turtlebot3_autorace_tunnel/CMakeLists.txt b/turtlebot3_autorace_tunnel/CMakeLists.txt new file mode 100644 index 0000000..5f22574 --- /dev/null +++ b/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() diff --git a/turtlebot3_autorace_tunnel/LICENSE b/turtlebot3_autorace_tunnel/LICENSE new file mode 100644 index 0000000..e69de29 diff --git a/turtlebot3_autorace_tunnel/launch/mission_tunnel.launch.py b/turtlebot3_autorace_tunnel/launch/mission_tunnel.launch.py new file mode 100644 index 0000000..fc788d5 --- /dev/null +++ b/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, + ]) diff --git a/turtlebot3_autorace_tunnel/map/map.pgm b/turtlebot3_autorace_tunnel/map/map.pgm new file mode 100644 index 0000000..bccb854 Binary files /dev/null and b/turtlebot3_autorace_tunnel/map/map.pgm differ diff --git a/turtlebot3_autorace_tunnel/map/map.yaml b/turtlebot3_autorace_tunnel/map/map.yaml new file mode 100644 index 0000000..cb89622 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/turtlebot3_autorace_tunnel/package.xml b/turtlebot3_autorace_tunnel/package.xml new file mode 100644 index 0000000..eb22657 --- /dev/null +++ b/turtlebot3_autorace_tunnel/package.xml @@ -0,0 +1,25 @@ + + + + turtlebot3_autorace_tunnel + 1.0.0 + ROS2 port of turtlebot3_autorace_tunnel + Pyo + Apache 2.0 + + ChanHyeong Lee + + ament_cmake + + rclpy + geometry_msgs + + ament_python + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/turtlebot3_autorace_tunnel/src/mission_tunnel.py b/turtlebot3_autorace_tunnel/src/mission_tunnel.py new file mode 100755 index 0000000..18a6b7d --- /dev/null +++ b/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()