앞서서 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()
  

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