[EN] Controller

Presentation

We aim to create a central node, called Controller. Its objective is regrouping all sensors data and decision making. It would allow making some form of user interface to display all this data, useful for future developments.

Node links (image in french for now)

Code overview

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

This snippet represents all declared node connexions (publish and 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
           

These two functions gather some topics contents to send them using the functions giveRGB() and 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
        
        

This has not been yet finalized, but it will allow the bot not to use the PID while it is not above a black guide line, and switch to SLAM. To do that, we check if the bot hasn't detected a blackline for a certain amount of time.

    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

The first functions redirects RGB values to the PID and the second one directs the bot using said PID.