개발환경

Desktop Ubuntu20.04 LTS

Raspberry pi 4b 4GB

ROS2 foxy

설명

미세먼지 측정 센서로부터 받아온 오염도 수치를 바탕으로 Fan을 제어한다.

오염도 수치에 따른 Fan모드는 다음과 같이 지정하였다.

저속모드 : 0

중속모드 : 1

고속모드 : 2

터보모드 : 3

 

현재는 미세먼지센서가 구비되어있지 않아 가상으로 토픽을 발행하여 테스트 하였다.

과정

1. 미세먼지 측정 노드로부터 0 ~ 3 까지의 오염도 수치를 서브스크라이브 한다.

2. 서브스크라이브한 오염도 수치를 바탕으로 Fan을 PWM제어 한다.

소스코드
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSDurabilityPolicy
from rclpy.qos import QoSHistoryPolicy
from rclpy.qos import QoSProfile
from rclpy.qos import QoSReliabilityPolicy
from rclpy.callback_groups import ReentrantCallbackGroup
from std_msgs.msg import Int16
import RPi.GPIO as GPIO

class FanPwmControl(Node):
    
    def __init__(self,channel):
        super().__init__('FanPwm')
        self.channel = channel 					//라즈베리파이 핀 넘버
        GPIO.setwarnings(False) 				//경고메세지 무시
        GPIO.setmode(GPIO.BOARD) 				//GPIO핀을 보드넘버로 읽기
        GPIO.setup(self.channel,GPIO.OUT) 		// 핀넘버 지정, 출력모드 지정
        self.pwm = GPIO.PWM(self.channel,2)		//핀 넘버의 PWM모드 활성화 
        self.pwm.start(10) 			//PWM 초기값 듀티비 10
        
        							//QOS프로파일 지정
        QOS_RKL10V = QoSProfile(
            reliability=QoSReliabilityPolicy.RELIABLE,
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=10,
            durability=QoSDurabilityPolicy.VOLATILE)
        							//오염도 수치 서브스크라이브 노드 생성    
        self.subscribe_pollu_grade = self.create_subscription(
            Int16,
            'pollu_grade',
            self.get_pollu_grade,
            QOS_RKL10V
            )
    								//서브스크라이브하며 처리할 함수
    def get_pollu_grade(self,msg):
        
        self.grade = msg.data
        self.pwm_mode(self.grade)
        self.pwm_info(self.grade)
        
    								//오염도 수치에따라 해당하는 메세지를 내보낸다.
    def pwm_info(self, grade):
        if grade == 0:
            self.get_logger().info('팬이 저속모드 입니다.')
        elif grade == 1:
            self.get_logger().info('팬이 중속모드 입니다.')
        elif grade == 2:
            self.get_logger().info('팬이 고속모드 입니다.')
        else:
            self.get_logger().info('팬이 터보모드 입니다.')  
    								//오염도 수치에 따라 해당하는 PWM 출력을 설정한다.
    def pwm_mode(self,grade):
        if grade == 0:
            self.pwm.ChangeDutyCycle(10)
        elif grade == 1:
            self.pwm.ChangeDutyCycle(30)
        elif grade == 2:
            self.pwm.ChangeDutyCycle(60)
        else:
            self.pwm.ChangeDutyCycle(90)

def main(args=None):
    rclpy.init(args=args)
    try:
        channel = 12 				//라즈베리파이의 PWM0 핀
        fan_mode = FanPwmControl(channel)
        try:
            rclpy.spin(fan_mode)
        except KeyboardInterrupt:
            fan_mode.get_logger().info('Keyboard Interrupt (SIGINT)')
        finally:
            fan_mode.destroy_node()
    finally:
        rclpy.shutdown()



if __name__ == '__main__':
    main()
결과

PWM모드의 변화를 유튜브에 업로드 했다,

ros2 topic pub /pollu_grade std_msgs/msg/Int16 "data: [원하는 오염도 수치 0 ~ 3]"

토픽을 수동발행하여 PWM제어를 한다.

 

https://www.youtube.com/watch?v=-jGOTXv73Yo&t=2s