[FR] Controller

Présentation

Nous souhaitons mettre en place une node centrale, appelée Controller, qui a pour objectif de centraliser les informations des capteurs ainsi que les prises de décisions. Et permettra de faire une interface utilisateur pour afficher toutes les infos pour un futur développement.

Communications de la node

Fonctionnement du code

def __init__(self):

        super().__init__('TortureBot4_Controleur_node')
        self.pid_order= Twist()
        self.rgb_value=rgbtype()
        self.order=Twist()
        self.control_PID=5;#todo put max value to begin with the LIDAR when PID test are finished
        
        #! [publisher]
        #* Publisher for the movement 
        self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)
        #* Publisher for the PID to send the RGB value 
        self.rgb_publisher= self.create_publisher(rgbtype, 'RGB_Value',1)
        
        
        
        #! [Subscription]
        #* Subscriber to the PID output 
        self.subscriptionPID = self.create_subscription(Twist,'PID_cmd_vel',self.get_PID_value,1)
        self.subscriptionPID
        #*Subscriber to the sensor topics
        self.sensors_sub =self.create_subscription(rgbtype,'tcs',self.getRGB,1)
        self.sensors_sub
        
        #![Timer]
        #* Timer to give movement order every 0.5 sec
        order_period = 0.5  # seconds
        self.timer = self.create_timer(order_period, self.control_order)
        self.timer = self.create_timer(order_period, self.giveRGB)
        
        self.i = 0

Cette partie représente toutes les connexions à la node qui sont déclarées (publish et subscribe).

    def getRGB(self,msgsub):# publish RGB value for the PID
        self.rgb_value=msgsub
        
    def get_PID_value(self, msgsub):# Get the PID output 
        self.pid_order=msgsub
           

Ces deux fonctions récupèrent le contenu de certains topics pour les renvoyer via les fonctions "giveRGB()" et "movement_order".

    def control_order(self):
        if (abs(self.pid_order.angular.z)>PI/2):#check if the PID is not out of range
            self.control_PID=min(5,self.control_PID+1)
        else:
            self.control_PID=0
        
        
        # Control wich means of localisation we use for the movement 
        if (self.control_PID<=5):#if the PID is out of range we don't use it
            self.order=self.pid_order
            self.movement_order()
        else:
            pass#todo Write for the lidar
        
        

Cette partie n'est pas finalisée, mais cela permet au robot de ne pas utiliser le PID quand il n'est pas sur une ligne noire afin de basculer sur le SLAM. Pour cela, on check si le robot n'a pas détecté la ligne noire depuis un certain temps.

    def giveRGB(self):
        self.rgb_publisher.publish(self.rgb_value)

    def movement_order(self): # Send the movement order 
        msg = Twist()
        msg.linear.x=self.order.linear.x; # mandatory to move forward or backward
        msg.angular.z=self.order.angular.z*PI# left or right
        self.publisher_.publish(msg)
        #self.get_logger().info('Publishing: "%s"' % msg.data)
        self.i += 1

La première fonction permet de renvoyer les valeurs RGB vers le PID et la deuxième de commander le bot avec les commandes du PID.