rosserial을 이용하여 로봇 이동 시 LED 자동 제어

 

- 개발 환경 : Raspberry Pi 4B (Ubuntu 18.04 / ROS melodic)

- 사용품 : md400T(모터드라이버), MDT150(인휠모터), ydlidar X4(라이다), 배터리, 

                IntelliThings iAHRS RB-SDA-v1(IMU센서), Arduino Mega, 

                WS2812 Flexible LED (https://www.devicemart.co.kr/goods/view?no=1328618#goods_file)

 

 

 

Step1. 아두이노 rosserial 환경 구축 및 LED 코드 작성

- 아래의 링크를 참고

   https://do-9un6.tistory.com/7

Arduino와 ROS로 Flexible LED 구동하기

 

- 기존에는 터미널 창을 열어 해당 명령어를 입력하여 ROS Serial 노드를 실행 및 LED 기능 제어를 함.

$ roscore
$ sudo chmod 766 /dev/ttyACM0
$ rosrun rosserial_python serial_node.py __name:=arduino _port:=/dev/ttyACM0 _baud:=57600

$ rostopic pub /switch std_msgs/UInt16MultiArray "{data: [1, 0, 0]}" --once

 

- 로봇이 움직일 때 LED가 자동으로 제어되도록 시도하고자 함.

LED_1.ino

0.05MB

 

LED_2.ino

0.05MB

 

Step2. 모터 드라이버(md) 패키지 파일 수정

- rostopic pub을 이용하여 Array 데이터를 publish하는 방식이였으므로, md 패키지의 cmd_main.cpp 파일에 해당 msg를 publish하도록 코드 추가 작성

 

- LED_1 : 정지 -> RED, 이동 -> GREEN

#include <std_msgs/UInt16MultiArray.h>

ros::Publisher switch_pub = nh.advertise<std_msgs::UInt16MultiArray>("switch", 10);
 
   std_msgs::UInt16MultiArray switch_msg;

        if(Md.sCmdLinearVel == 0 && Md.sCmdAngularVel == 0)
        {
            switch_msg.data = {0, 0, 1};
        }
        else
        {
            switch_msg.data = {1, 0, 0};
        }

        switch_pub.publish(switch_msg);            //Publish the message 'switch(led)'

 

- LED_2 : 정지 -> RED, 이동 -> GREEN, 좌우 회전 -> GREEN 깜빡이

#include <std_msgs/UInt16MultiArray.h>

ros::Publisher switch_pub = nh.advertise<std_msgs::UInt16MultiArray>("switch", 10);
 
   std_msgs::UInt16MultiArray switch_msg;

         if(Md.sCmdLinearVel == 0.0 && Md.sCmdAngularVel == 0.0)
         {
             switch_msg.data = {0, 0, 1};
         }
         else if(Md.sCmdAngularVel > 0.0)
         {
             switch_msg.data = {0, 1, 0};
         }
         else if(Md.sCmdAngularVel < 0.0)
         {
             switch_msg.data = {0, 1, 1};
         }
         else
         {
             switch_msg.data = {1, 0, 0};
         }

    switch_pub.publish(switch_msg);            //Publish the message 'switch(led)'

 

 

Step3. remote controll 시에 정상 작동하는지 확인

- 아래의 코드 입력 후 키보드 제어 시 움직일 때, LED가 자동으로 바뀌는지 확인하기

  (정지 -> Red, 이동 -> Green)

$ roslaunch md md.launch
$ rosrun rosserial_python serial_node.py __name:=arduino _port:=/dev/ttyACM0 _baud:=57600
$ roslaunch morobot_teleop morobot_teleop_key.launch

 

Step4. Navigation 에 적용하기

- 먼저 bringup.launch 파일에 rosserial 실행 코드를 추가

 $ cd ~/catkin_ws/src/morobot/morobot_bringup/launch

// morobot_G2.launch 파일 맨 밑에 아래의 코드 추가
 
 <!-- Arduino LED -->
  <node name="serial_node" pkg="rosserial_python" type="serial_node.py">
    <param name="port" value="/dev/ttyACM0"/>
    <param name="baud" value="57600"/>
  </node>

 

- 내비게이션 실행 및 LED 자동 제어 확인

$ roslaunch morobot_bringup morobot_G2.launch
$ roslaunch morobot_navigation morobot_navigation_G2.launch
$ roslaunch morobot_navigation morobot_navigation_rviz.launch

 

- github에 코드 업로드 함.

$ git clone https://github.com/odk9611/morobot_UCP_Beta_LED.git

 

 

 

 

 

* Run loop error: [Error 5] Input/output error 발생

[WARN] [1545576797.168735]: Run loop error: [Error 5] Input/output error
Traceback (most recent call last):
  File "/home/catkin_ws/src/rosserial/rosserial_python/nodes/serial_node.py", line 89, in <module>
    client.run()
  File "/home/wolf/Mechanical_Practice/catkin_ws/src/rosserial/rosserial_python/SerialClient.py", line 554, in run
    self.port.flushInput()
  File "/usr/lib/python2.7/dist-packages/serial/serialutil.py", line 532, in flushInput
    self.reset_input_buffer()
  File "/usr/lib/python2.7/dist-packages/serial/serialposix.py", line 566, in reset_input_buffer
    termios.tcflush(self.fd, termios.TCIFLUSH)
termios.error: (5, 'Input/output error')

 

- 해당 에러가 발생하는 이유는 전력 부족 문제일 가능성이 높음 -> LED 밝기를 255->50으로 낮춤

 

https://answers.ros.org/question/311408/rosserial-kinetic-error-5-inputoutput-error/