WSL2 ROS2를 이용한 라즈베리파이 RGB LED 제어하기

https://doyun98.tistory.com/132

WSL2에 Docker ROS2 환경 구축하기

 

앞서 설치한 환경에서의 ROS2 노드에서 라즈베리파이의 RGB LED를 소켓 통신으로 제어해보겠습니다.

라즈베리파이 LED 서버 구현

회로 구성

• RED LED → GPIO 17
• GREEN LED → GPIO 27
• BLUE LED → GPIO 22
• 공통 GND

LED 서버 코드

led_server.cpp:

#include <iostream>
#include <cstring>
#include <unistd.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <wiringPi.h>
#include <softPwm.h>

#define PORT 8888
#define RED_PIN 17
#define GREEN_PIN 27
#define BLUE_PIN 22

class RGBLEDServer {
private:
    int server_fd;

    void setupGPIO() {
        wiringPiSetupGpio();
        softPwmCreate(RED_PIN, 0, 100);
        softPwmCreate(GREEN_PIN, 0, 100);
        softPwmCreate(BLUE_PIN, 0, 100);
        std::cout << "GPIO 초기화 완료" << std::endl;
    }

    void setupSocket() {
        struct sockaddr_in address;
        int opt = 1;

        server_fd = socket(AF_INET, SOCK_STREAM, 0);
        setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));

        address.sin_family = AF_INET;
        address.sin_addr.s_addr = INADDR_ANY;
        address.sin_port = htons(PORT);

        bind(server_fd, (struct sockaddr*)&address, sizeof(address));
        listen(server_fd, 3);

        std::cout << "LED 서버 시작 (포트: " << PORT << ")" << std::endl;
    }

    void setColor(int r, int g, int b) {
        softPwmWrite(RED_PIN, r);
        softPwmWrite(GREEN_PIN, g);
        softPwmWrite(BLUE_PIN, b);
        std::cout << "RGB(" << r << ", " << g << ", " << b << ")" << std::endl;
    }

public:
    RGBLEDServer() {
        setupGPIO();
        setupSocket();
    }

    void run() {
        while (true) {
            struct sockaddr_in client_addr;
            socklen_t addr_len = sizeof(client_addr);
            int client_fd = accept(server_fd, (struct sockaddr*)&client_addr, &addr_len);

            std::cout << "클라이언트 연결됨" << std::endl;

            char buffer[1024];
            while (true) {
                int bytes = read(client_fd, buffer, sizeof(buffer) - 1);
                if (bytes <= 0) break;

                buffer[bytes] = '\0';

                int r, g, b;
                if (sscanf(buffer, "%d,%d,%d", &r, &g, &b) == 3) {
                    setColor(r, g, b);
                }
            }

            close(client_fd);
            std::cout << "클라이언트 연결 종료" << std::endl;
        }
    }
};

int main() {
    RGBLEDServer server;
    server.run();
    return 0;
}

컴파일 및 실행

# 라즈베리파이에서
g++ -o led_server led_server.cpp -lwiringPi -lpthread
sudo ./led_server

출력:

GPIO 초기화 완료
LED 서버 시작 (포트: 8888)

ROS2 LED 제어 노드 구현

패키지 생성

# Docker 컨테이너 안에서
cd /ros2_ws/src
ros2 pkg create --build-type ament_cmake led_controller \
  --dependencies rclcpp std_msgs geometry_msgs

노드 코드

src/led_controller/src/rgb_led_controller.cpp:

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/color_rgba.hpp>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <cstring>

class RGBLEDController : public rclcpp::Node {
private:
    rclcpp::Subscription<std_msgs::msg::ColorRGBA>::SharedPtr sub_;
    int sock_;
    std::string raspberry_pi_ip_;
    int port_;

    void connectToRaspberryPi() {
        sock_ = socket(AF_INET, SOCK_STREAM, 0);

        struct sockaddr_in server_addr;
        server_addr.sin_family = AF_INET;
        server_addr.sin_port = htons(port_);
        inet_pton(AF_INET, raspberry_pi_ip_.c_str(), &server_addr.sin_addr);

        if (connect(sock_, (struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) {
            RCLCPP_ERROR(this->get_logger(), "라즈베리파이 연결 실패");
            return;
        }

        RCLCPP_INFO(this->get_logger(), "라즈베리파이 연결 성공 (%s:%d)", 
                    raspberry_pi_ip_.c_str(), port_);
    }

    void colorCallback(const std_msgs::msg::ColorRGBA::SharedPtr msg) {
        int r = static_cast<int>(msg->r * 100);
        int g = static_cast<int>(msg->g * 100);
        int b = static_cast<int>(msg->b * 100);

        char cmd[64];
        snprintf(cmd, sizeof(cmd), "%d,%d,%d", r, g, b);
        send(sock_, cmd, strlen(cmd), 0);

        RCLCPP_INFO(this->get_logger(), "RGB(%d, %d, %d) 전송", r, g, b);
    }

public:
    RGBLEDController() : Node("rgb_led_controller") {
        this->declare_parameter("raspberry_pi_ip", "192.168.0.13");
        this->declare_parameter("port", 8888);

        raspberry_pi_ip_ = this->get_parameter("raspberry_pi_ip").as_string();
        port_ = this->get_parameter("port").as_int();

        connectToRaspberryPi();

        sub_ = this->create_subscription<std_msgs::msg::ColorRGBA>(
            "rgb_led/color", 10,
            std::bind(&RGBLEDController::colorCallback, this, std::placeholders::_1)
        );

        RCLCPP_INFO(this->get_logger(), "RGB LED Controller 시작");
    }

    ~RGBLEDController() {
        close(sock_);
    }
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<RGBLEDController>());
    rclcpp::shutdown();
    return 0;
}

CMakeLists.txt 설정

cmake_minimum_required(VERSION 3.8)
project(led_controller)

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)

add_executable(rgb_led_controller src/rgb_led_controller.cpp)
ament_target_dependencies(rgb_led_controller
  rclcpp
  std_msgs
  geometry_msgs
)

install(TARGETS
  rgb_led_controller
  DESTINATION lib/${PROJECT_NAME}
)

ament_package()

빌드 및 실행

# Docker 컨테이너 안에서
cd /ros2_ws
colcon build --packages-select led_controller
source install/setup.bash

# 노드 실행
ros2 run led_controller rgb_led_controller --ros-args -p raspberry_pi_ip:=192.168.0.13

 

LED 제어 테스트

토픽으로 색상 전송

# 빨간색
ros2 topic pub --once /rgb_led/color std_msgs/msg/ColorRGBA "{r: 1.0, g: 0.0, b: 0.0, a: 1.0}"

# 초록색
ros2 topic pub --once /rgb_led/color std_msgs/msg/ColorRGBA "{r: 0.0, g: 1.0, b: 0.0, a: 1.0}"

# 파란색
ros2 topic pub --once /rgb_led/color std_msgs/msg/ColorRGBA "{r: 0.0, g: 0.0, b: 1.0, a: 1.0}"

# 흰색
ros2 topic pub --once /rgb_led/color std_msgs/msg/ColorRGBA "{r: 1.0, g: 1.0, b: 1.0, a: 1.0}"

토픽 확인

# 토픽 목록
ros2 topic list

# 토픽 정보
ros2 topic info /rgb_led/color

# 토픽 모니터링
ros2 topic echo /rgb_led/color