przyklad_ czyszczenie

usluga_ros2.py

### podstawowe przyklady do rozwiniecia



from rclpy.node import Node


from geometry_msgs.msg import Twist

from turtlesim.msg import Pose
from turtlesim.srv import SetPen
from std_srvs.srv import Empty


class TurtleMover(Node): #here we define new node
    kolorek = "zielony"
    def __init__(self): 
        super().__init__('zolwik_przyklad')
       
        self.publisher_ = self.create_publisher(Twist, 
                                                'turtle1/cmd_vel',
                                                10)

        
        
        self.subscriber = self.create_subscription(
    Pose,
    "turtle1/pose", self.subscription_callback,10) #tu subskrypcja na pozycje
        

        self.service_client = self.create_client(SetPen,
                            "turtle1/set_pen")
        
        self.czyszczyciel = self.create_client(Empty,
                            "/clear")

    def subscription_callback(self,pose):
        
        if (pose.x > 5.5 and self.kolorek == "zielony"):
            
            new_pen = SetPen.Request() 

            new_pen.r = 255
            new_pen.width = 5 
            self.service_client.call_async(new_pen)
            self.kolorek = "czerwony"
        elif (pose.x<= 5.5 and self.kolorek == "czerwony"):
            new_pen = SetPen.Request() 

            new_pen.r = 0
            new_pen.g = 255
            new_pen.width = 5 
            self.service_client.call_async(new_pen)            
            self.kolorek = "zielony"
            
        if pose.x > 9 or pose.x < 1 or pose.y > 9 or pose.x < 1:
            clear_req = Empty.Request()
            self.czyszczyciel.call_async(clear_req)
    
    def move_turtle(self,twist_command): #we add helper function
        self.publisher_.publish(twist_command)