리스너 작성하기
저작권: 쿼드(QUAD) 드론연구소 https://smartstore.naver.com/maponarooo / Updated : 2024-10-11
1. 리스너 노드 작성
먼저 소스 파일 learning_tf2_py을 만들어 보겠습니다. 이전 튜토리얼에서 만든 패키지로 이동합니다. src/learning_tf2_py/learning_tf2_py디렉토리 내부에서 다음 명령을 입력하여 예제 리스너 코드를 다운로드합니다.
wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/turtle_tf2_listener.py편집기를 사용하여 turtle_tf2_listener.py 파일을 엽니다.
import math
from geometry_msgs.msg import Twist
import rclpy
from rclpy.node import Node
from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
from turtlesim.srv import Spawn
class FrameListener(Node):
def __init__(self):
super().__init__('turtle_tf2_frame_listener')
# Declare and acquire `target_frame` parameter
self.target_frame = self.declare_parameter(
'target_frame', 'turtle1').get_parameter_value().string_value
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
# Create a client to spawn a turtle
self.spawner = self.create_client(Spawn, 'spawn')
# Boolean values to store the information
# if the service for spawning turtle is available
self.turtle_spawning_service_ready = False
# if the turtle was successfully spawned
self.turtle_spawned = False
# Create turtle2 velocity publisher
self.publisher = self.create_publisher(Twist, 'turtle2/cmd_vel', 1)
# Call on_timer function every second
self.timer = self.create_timer(1.0, self.on_timer)
def on_timer(self):
# Store frame names in variables that will be used to
# compute transformations
from_frame_rel = self.target_frame
to_frame_rel = 'turtle2'
if self.turtle_spawning_service_ready:
if self.turtle_spawned:
# Look up for the transformation between target_frame and turtle2 frames
# and send velocity commands for turtle2 to reach target_frame
try:
t = self.tf_buffer.lookup_transform(
to_frame_rel,
from_frame_rel,
rclpy.time.Time())
except TransformException as ex:
self.get_logger().info(
f'Could not transform {to_frame_rel} to {from_frame_rel}: {ex}')
return
msg = Twist()
scale_rotation_rate = 1.0
msg.angular.z = scale_rotation_rate * math.atan2(
t.transform.translation.y,
t.transform.translation.x)
scale_forward_speed = 0.5
msg.linear.x = scale_forward_speed * math.sqrt(
t.transform.translation.x ** 2 +
t.transform.translation.y ** 2)
self.publisher.publish(msg)
else:
if self.result.done():
self.get_logger().info(
f'Successfully spawned {self.result.result().name}')
self.turtle_spawned = True
else:
self.get_logger().info('Spawn is not finished')
else:
if self.spawner.service_is_ready():
# Initialize request with turtle name and coordinates
# Note that x, y and theta are defined as floats in turtlesim/srv/Spawn
request = Spawn.Request()
request.name = 'turtle2'
request.x = float(4)
request.y = float(2)
request.theta = float(0)
# Call request
self.result = self.spawner.call_async(request)
self.turtle_spawning_service_ready = True
else:
# Check if the service is ready
self.get_logger().info('Service is not ready')
def main():
rclpy.init()
node = FrameListener()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
rclpy.shutdown()1.1 코드 검토
이제 프레임 변환에 액세스하는 데 관련된 코드를 살펴보겠습니다. 이 tf2_ros패키지는 변환 수신 작업을 더 쉽게 만드는 데 도움이 되는 TransformListener의 구현을 제공합니다 .
from tf2_ros.transform_listener import TransformListener여기서 TransformListener객체를 만듭니다. 리스너가 만들어지면 와이어를 통해 tf2 변환을 수신하기 시작하고 최대 10초 동안 버퍼링합니다.
self.tf_listener = TransformListener(self.tf_buffer, self)마지막으로, 우리는 리스너에 특정 변환을 쿼리합니다. 우리는 다음 인수로 lookup_transform메서드를 호출합니다.
타겟 프레임
소스 프레임
우리가 변형하고 싶은 시간
rclpy.time.Time()은사용 가능한 최신 변환만 제공됩니다. 이 모든 것은 가능한 예외를 처리하기 위해 try-except 블록에 싸여 있습니다.
t = self.tf_buffer.lookup_transform(
to_frame_rel,
from_frame_rel,
rclpy.time.Time())1.2 진입점 추가
명령이 노드를 실행하도록 하려면 ( 디렉토리에 있는)에 진입점을 추가해야 합니다.ros2 runsetup.pysrc/learning_tf2_py
다음 줄을 'console_scripts':괄호 안에 추가하세요.
'turtle_tf2_listener = learning_tf2_py.turtle_tf2_listener:main',디렉토리 src/learning_tf2_py/launch에서 호출된 turtle_tf2_demo.launch.py launch 파일을 텍스트 편집기로 열고 launch 설명에 두 개의 새 노드를 추가하고 launch 인수를 추가하고 import를 추가합니다. 결과 파일은 다음과 같아야 합니다.
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='turtlesim',
executable='turtlesim_node',
name='sim'
),
Node(
package='learning_tf2_py',
executable='turtle_tf2_broadcaster',
name='broadcaster1',
parameters=[
{'turtlename': 'turtle1'}
]
),
DeclareLaunchArgument(
'target_frame', default_value='turtle1',
description='Target frame name.'
),
Node(
package='learning_tf2_py',
executable='turtle_tf2_broadcaster',
name='broadcaster2',
parameters=[
{'turtlename': 'turtle2'}
]
),
Node(
package='learning_tf2_py',
executable='turtle_tf2_listener',
name='listener',
parameters=[
{'target_frame': LaunchConfiguration('target_frame')}
]
),
])이렇게 하면 target_frame인수가 선언되고, 생성할 두 번째 거북이에 대한 브로드캐스터와 해당 변환을 구독할 리스너가 시작됩니다.
3. 빌드
작업 공간 루트에서 rosdep실행하여 누락된 종속성을 확인합니다.
cd ~/ros2_ws
rosdep install -i --from-path src --rosdistro humble -y작업 공간 루트에서 패키지를 빌드하세요.
colcon build --packages-select learning_tf2_py새 터미널을 열고 작업 공간 루트로 이동한 후 설치 파일을 소싱합니다.
. install/setup.bash4. 실행
이제 전체 거북이 데모를 시작할 준비가 되었습니다.
ros2 launch learning_tf2_py turtle_tf2_demo.launch.py거북이 두 마리가 있는 거북이 시뮬레이션이 보일 것입니다. 두 번째 터미널 창에서 다음 명령을 입력하세요:
ros2 run turtlesim turtle_teleop_key모든 것이 제대로 작동하는지 확인하려면 화살표 키를 사용하여 첫 번째 거북이 주위를 돌아다니세요(시뮬레이터 창이 아닌 터미널 창이 활성화되어 있는지 확인하세요).그러면 첫 번째 거북이 뒤에 두 번째 거북이가 나오는 것을 보실 수 있습니다!
Last updated