ROS Noetic : Server & Client

앞서서 ROS service를 활용해보았다. 이는 서버-클라이언트 구조와 흡사함을 알 수 있다. 이번에는 실제로 서버-클라이언트의 구조를 만들어본다.

일단은 앞서 만들었던 closed_loop의 turtle_controller를 이용하며 화면의 좌측에 있으면 붉은 색, 우측에 있으면 녹색의 자취를 나타내는 형태를 만들 것이다.

이전 코드를 가져와서 bold 처리된 줄들을 추가해준다.

#!/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) 

        **# new function to call service /turtle1/set_pen
    def call_set_pen_service(r=0, g=0, b=0, width=4, off=0):
        try:
            # this is how we create service client : request
            set_pen = rospy.ServiceProxy("/turtle1/set_pen", SetPen)
            response = set_pen(r,g,b,width,off)
        # if service doesn't work we get exception
        except rospy.ServiceException as e:
            rospy.logwarn(e)**

        # we want to publish cmd_vel topic through this callback function
    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 = 1.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")
        **# wait for service to be called
    rospy.wait_for_service("/turtle1/set_pen")**

    control_node = TurtleControl()
    rospy.spin()

• 일단 service를 불러내기 위한 새로운 함수를 정의한다. parameter는 불러올 service의 parameter와 동일하게 맞춰주었다.

  try - except 구조로 rospy.ServiceProxy("service name", service type) 형태에 맞춘다.

  

  위 예시의 경우 rosservice로 확인하고 맞춰주었다.

  만약 service를 불러오지 못하면 exception을 토한다→except rospy.ServiceException as e

• 다만! 이 줄은 pose callback처럼 자주 부를 필요가 없다.

    #pose callback 안에 있음
    if rcvmsg.x > 5.5:
            self.call_set_pen_service(255,0,0,4,0)
    else : 
            self.call_set_pen_service(0,255,0,4,0)

  따라서 이전의 x 좌표를 저장해주는 다른 변수를 만들어주고 현재와 비교해서 기준인 5.5를 지나갈 때만 색을 바꿔주는 모양으로 가본다.

    # __init__ 안에
    self.prev_x =0
  
    if rcvmsg.x > 5.5 and self.prev_x < 5.5:
            prev_x = rcvmsg.x
            self.call_set_pen_service(255,0,0,4,0)
    elif rcvmsg.x < 5.5 and self.prev_x > 5.5:
            prev_x = rcvmsg.x 
            self.call_set_pen_service(0,255,0,4,0)

  이러면 변화가 없을 때에는 불필요한 컴퓨팅을 하지 않아도 된다.

  전체 코드는 아래와 같다.

    #!/usr/bin/env python3
    import rospy
    from turtlesim.msg import Pose # output of turtlesim
    from geometry_msgs.msg import Twist # input for turtlesim
    from turtlesim.srv import SetPen
  
    class TurtleControl:
        def __init__(self):
            rospy.init_node('turtle_controller')
            self.prev_x =0
            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) 
  
        # new function to call service /turtle1/set_pen
        def call_set_pen_service(self, r=0, g=0, b=0, width=4, off=0):
            try:
                # this is how we create service client : request
                set_pen = rospy.ServiceProxy("/turtle1/set_pen", SetPen)
                response = set_pen(r,g,b,width,off)
            # if service doesn't work we get exception
            except rospy.ServiceException as e:
                rospy.logwarn(e)
  
        # we want to publish cmd_vel topic through this callback function
        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 = 1.0
                self.cmdmsg.angular.z = 1.8
            else:
                self.cmdmsg.linear.x = 4.0
                self.cmdmsg.angular.z = 0.0
  
            if rcvmsg.x > 5.5 and self.prev_x < 5.5:
                self.prev_x = rcvmsg.x
                rospy.loginfo("set color red")
                self.call_set_pen_service(255,0,0,4,0)
            elif rcvmsg.x < 5.5 and self.prev_x > 5.5:
                self.prev_x = rcvmsg.x 
                rospy.loginfo("set color green")
                self.call_set_pen_service(0,255,0,4,0)    
            self.pub.publish(self.cmdmsg)
  
    if __name__=='__main__':
        rospy.loginfo("control node init")
        # wait for service to be called
        rospy.wait_for_service("/turtle1/set_pen")
  
        control_node = TurtleControl()
        rospy.spin()

  위를 시행하면 다음과 같은 화면 결과를 볼 수 있다!!