이번 시간에는 우리 turtlesim 거북이를 벽에 부딪히지 않고 자율 주행을 하도록 만들어 보겠습니다.
앞서 배웠던 /turtle1/pose 토픽의 pose.x, pose.y 메시지를 확인해서 벽에 근접하면 자동으로 거북이가 벽을 피해 움직이도록 할 생각 입니다. 그러자면 우선 거북이의 위치와 벽의 거리를 먼저 확인해야 합니다.
앞에서 만든 pose_subscriber 노드를 이용해 거북이의 x, y 좌표를 알 수 있습니다. 또한, 거북이를 움직여서 벽의 x, y 좌표도 알 수 있습니다.
이제 우리가 움직일 거북이의 이동 좌표를 알았습니다. 우리는 거북이가 흰색 팬스 안에서만 움직이도록 조절 하면 됩니다. 이제 코딩을 할 시간 입니다.
코드 작성
우선 코드를 작성할 turtle_controller.py 파일을 만들고 실행 권한을 부여 합니다.
$ cd ~/ros2_ws/src/my_robot_controller/my_robot_controller
$ touch turtle_controller.py
$ $ chmod +x turtle_controller.py
vscode를 이용하여 코드를 작성 합니다.
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from turtlesim.msg import Pose
from geometry_msgs.msg import Twist
class TurtleControllerNode(Node):
def __init__(self):
super().__init__("turtle_controller")
self.cmd_vel_publisher_ = self.create_publisher(Twist, "/turtle1/cmd_vel", 10)
self.pose_subscriber_ = self.create_subscription(Pose, "/turtle1/pose", self.pose_callback, 10)
self.get_logger().info("Turtle controller node has been started.")
def pose_callback(self, pose: Pose):
cmd = Twist()
if pose.x > 9.0 or pose.x < 2.0 or pose.y > 9.0 or pose.y < 2.0:
cmd.linear.x = 1.0
cmd.angular.z = 1.0
else:
cmd.linear.x = 5.0
cmd.angular.z = 0.0
self.cmd_vel_publisher_.publish(cmd)
def main(args=None):
rclpy.init(args=args)
node = TurtleControllerNode()
rclpy.spin(node)
rclpy.shutdown()
코드 분석
main() 문은 동일한 구조 입니다.
def main(args=None):
rclpy.init(args=args)
node = TurtleControllerNode()
rclpy.spin(node)
rclpy.shutdown()
노드 클래스에서 /turtule1/pose 토픽을 구독하고 /turtle1/cmd_vel 토픽을 발행하는 발행자와 구독자를 생성해 줍니다.
self.cmd_vel_publisher_ = self.create_publisher(Twist, "/turtle1/cmd_vel", 10)
self.pose_subscriber_ = self.create_subscription(Pose, "/turtle1/pose", self.pose_callback, 10)
self.get_logger().info("Turtle controller node has been started.")
구독자 pose_subscriber_ 에서 pose_callback 함수를 가동 시키므로 콜백 함수에서 거북이의 움직임을 제어 하도록 합니다.
벽에 부딪히지 않도록 pose.x 와 pose.y 를 조건문으로 처리해 주고 마지막으로 cmd_vel_publisher_ 발행자에 cmd_vel 토픽을 발행 시킵니다.
def pose_callback(self, pose: Pose):
cmd = Twist()
if pose.x > 9.0 or pose.x < 2.0 or pose.y > 9.0 or pose.y < 2.0:
cmd.linear.x = 1.0
cmd.angular.z = 1.0
else:
cmd.linear.x = 5.0
cmd.angular.z = 0.0
self.cmd_vel_publisher_.publish(cmd)
entry_points 등록
이제 코딩이 끝났습니다. 테스트를 하기 전에 setup.py의entry_point에 우리가 새로 작성한 노드 프로그램을 등록해 주어야 합니다.
entry_points={
'console_scripts': [
"test_node = my_robot_controller.my_first_node:main",
"draw_circle = my_robot_controller.draw_circle:main",
"pose_subscriber = my_robot_controller.pose_subscriber:main",
"turtle_controller = my_robot_controller.turtle_controller:main"
],
빌드
$ colcon build --symlink-install
실행
새로운 작업 환경을 소싱하고 turtlesim 노드를 기동 시킵니다.
$ source ~/ros2_ws/install/setup.bash
$ ros2 run turtlesim turtlesim_node
새로운 터미널을 열고 역시 작업 환경을 소싱하고 turtle_controller 노드를 실행 합니다.
$ $ source ~/ros2_ws/install/setup.bash
$ ros2 run my_robot_controller turtle_controller
이제 거북이가 벽에 부딪히지 않고 자율 주행 하는 것을 볼 수 있습니다.
