ROS Noetic : Closed Loop system with Turtlesim

이번에는 한 node 안에 publisher와 subscriber가 모두 있는 closed loop system을 만들어본다.

역시나 tutlesim 이용할거다. 일단 한 방향으로 직진하는 모양으로 만들었다.

이전과 같이 scripts폴더 안에 turtle_controller.py를 생성한다. 점점 복잡해질 수 있다 판단하여 class 안에 집어넣어 버렸다. 따로 만들어줘도 상관은 없을 듯 하다.

#!/usr/bin/env python3
import rospy
from turtlesim.msg import Pose # output of turtlesim
from geometry_msgs.msg import Twist # input for turtlesim

class TurtleControl:
    def __init__(self):
        rospy.init_node('turtle_controller')
        self.cmdmsg = Twist()
        self.pub = rospy.Publisher("/turtle1/cmd_vel", Twist, queue_size=10) 
        self.sub = rospy.Subscriber("/turtle1/pose", Pose, callback = self.pose_callback) 

    # we want to publish cmd_vel topic through this callback function
    def pose_callback(self,rcvmsg: Pose):
        self.cmdmsg.linear.x = 5
        self.cmdmsg.angular.z = 0
        self.pub.publish(self.cmdmsg)

if __name__=='__main__':
    rospy.loginfo("control node init")
    control_node = TurtleControl()
    rospy.spin()

잊지 말고 실행 권한을 주자;;

sudo chmod +x ../turtle_controller.py

실행을 해보면 rqt_graph상 이미지와 같이 나타난다.

turtle_controller node가 turtlesim에서 turtle1/pose를 받아오고 turtle1/cmd_vel을 보내주는 것을 확인할 수 있다.

이 거북이가 화면의 벽에 부딪히면 튕겨나오는 pub-sub 프로그램을 만들 것이다. 크게 보면 자율주행에서의 장애물 피하기 느낌이다. 이를 위해서 사각형 boundary의 모서리 좌표를 확인해야 한다. teleop_key로 왔다갔다 하면서 (0.0, 0.0)에서 (11.9, 11.9)정도임을 확인했다.

위 프로그램을 수정해준다.

#!/usr/bin/env python3
import rospy
from turtlesim.msg import Pose # output of turtlesim
from geometry_msgs.msg import Twist # input for turtlesim

class TurtleControl:
    def __init__(self):
        rospy.init_node('turtle_controller')
        self.cmdmsg = Twist()
        self.pub = rospy.Publisher("/turtle1/cmd_vel", Twist, queue_size=10) 
        self.sub = rospy.Subscriber("/turtle1/pose", Pose, callback = self.pose_callback) 

    def pose_callback(self,rcvmsg: Pose):
        if rcvmsg.x > 9.0 or rcvmsg.x < 2.0 or rcvmsg.y > 9.0 or rcvmsg.y < 2.0:
            self.cmdmsg.linear.x = 2.0
            self.cmdmsg.angular.z = 1.8
        else:
            self.cmdmsg.linear.x = 4.0
            self.cmdmsg.angular.z = 0.0

        self.pub.publish(self.cmdmsg)

if __name__=='__main__':
    rospy.loginfo("control node init")
    control_node = TurtleControl()
    rospy.spin()

중간 if문을 확인하면 직진하다가 벽에 가까워지면 회전하는 모양을 보인다.