[ROS] 10. ROS + Arduino 와 통신 TTL to RS485

☞ 메인보드 : Jetson Nano Developer Kit
☞ 운영 체제 : Ubuntu 18.04 - JetPack 4.4.1
☞ ROS 버전 : Melodic
☞ IDE : Visual Studio Code & Arduino
☞ 언어 : C++

---

목차

○ 1. Jetson Nano (Service Client)
○ 2. Arduino (Service Server)
○ 3. 실행 결과

---

 

아직 아두이노를 설치하지 않았다면 아래의 포스트를 읽고 설치하면 된다.

[ROS] 8. ROS + Arduino 설치하여 Rosserial 사용해보기

이번 포스트는 위와 같은 느낌으로 진행해보려고 한다. 아래에 첨부한 사진은 섹시한 액츄에이터 누리로봇과 아두이노 사이에 흐름제어를 맡아줄 RS485 컨버터이다. 빨갱이는 5~6천원이면 샀던거 같고 파랭이는 35~40만원 정도 했던거로 기억한다.

Converter & Nuri Robot

① Jetson Nano (Service Client)

서비스 통신을 하기 위해서 패키지를 하나 생성하고 바로 작업에 들어가기 위해서 catkin_make를 실행한다.

 

catkin_create_pkg rs485 roscpp std_msgs message_generation catkin_make

※ 서비스 파일 (num.srv)

디렉터리 ☞ ~/catkin_ws/srcs/service/srv/msgType.srv

 

uint8 modeNumber /// 요청(request) 
---
uint8 result /// 응답(response)

서비스 메시지 타입과 이름을 정의하여 넣는다. RS485라는 프로젝트 디렉토리안에 srv라는 새 디렉토리를 만들어 그 안에 num.srv라는 이름의 서비스 파일을 생성하여 넣어주었다. 해당 서비스 파일을 참고하기 위해서 client.cpp에서는 #include "rs485/num.h"로 정의하여 num.srv에 저장된 커스텀 메시지 변수들을 불러와 사용할 수 있다. CMakeLists.txt에서 add_service_files(FILES [메시지명.srv])로 적어 넣어주어야 해당 프로젝트에서 사용하는 서비스 메시지로 인식이 된다.

※ 코드 작성 (client.cpp)

~/catkin_ws/src/rs485/src 에서 코드 작성을 위한 파일 하나를 생성해준다. ROS 작업을 할 때 사용하는 IDE 중에 가장 많이 사용되는건 QtCreator인데 ROS Kinetic 버전에 해당 IDE를 ROS와 연동할 수 있는 플러그인이 존재했기 때문이다. [깃허브 닉네임 암스트롱이었나?] 아쉽게도 Melodic에는 플러그인이 없고 Jetson Nano는 AMD64 아키텍쳐 기반이므로 호환되는 소프트웨어가 적다. 이런 문제점은 젯슨 포럼이나 깃허브 이슈에서 많이 등장하기 때문에 호환성에 대한 문제는 곧 해결되지 않을까 기대한다. 무튼 QtCreator의 장점인 ROS 플러그인을 사용할 수 없어 Gedit 편집기를 애용하다가 AMD64에서 사용할 수 있는 편집 Visual Studio Code 버전을 만든 사람이 있어서 사용하고 있다.

#include "ros/ros.h"
#include "rs485/msgType.h"
int main(int argc, char** argv){
	ros::init(argc, argv, "client2");
	ros::NodeHandle nh;
	ros::ServiceClient
	client = nh.serviceClient<rs485::msgType>("rs485");
	rs485::msgType srv;
	for(int i = 0; i < 11; i++){
		srv.request.x = i;
		if(client.call(srv)){
			ROS_INFO("request %d", srv.request.x);
			ROS_INFO("response %d", srv.response.result);
		} else { 
			ROS_INFO("ERROR");
			return 1;
		} 
	} 
	return 0;
}

 rs485::num srv; 
 for(int i = 0; i < 11; i++){ srv.request.modeNumber = i; }

 

위 코드 중에서 이 부분이 Client(Jetson)가 Server(Arduino)에 요청을 보내는 내용이다.
누리로봇은 16진수에의해 작동하며 총 11가지의 정보에 대해서 메시지를 전송해야하기 때문에 for문으로 돌리는 방식을 택했다.

※ CMAKE (CMakeLists.txt)

 

cmake_minimum_required(VERSION 3.0.2)
project(rs485)
find_package(catkin REQUIRED COMPONENTS roscpp std_msgs message_generation)
add_service_files(FILES num.srv)
generate_messages(DEPENDENCIES std_msgs)
catkin_package( 
	# INCLUDE_DIRS include 
	# LIBRARIES rs485 
	# CATKIN_DEPENDS roscpp std_msgs 
	# DEPENDS system_lib
) 
include_directories( 
	# include 
	${catkin_INCLUDE_DIRS}
) 
add_executable(${PROJECT_NAME} src/client.cpp) 
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 
add_dependencies(${PROJECT_NAME} rs485_generate_messages_cpp)

 

※ XML (package.xml)

 

<?xml version="1.0"?> 
<package format="2"> 
<name>rs485</name>
<version>0.0.0</version> 
<description>The rs485 package</description> 
<maintainer email="yasun95@todo.todo">yasun95</maintainer> 
<license>TODO</license> 
<buildtool_depend>catkin</buildtool_depend> 
<build_depend>roscpp</build_depend> 
<build_depend>std_msgs</build_depend>
<build_depend>message_generation</build_depend> 
<build_export_depend>roscpp</build_export_depend> 
<build_export_depend>std_msgs</build_export_depend> 
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>message_runtime</exec_depend>
<export> </export>
</package>

모든 작업을 마쳤으면

 

catkin_make

 

 

---

② Arduino (Service Server)

 

RS485 라이브러리 다운로드

Gammon Forum : Electronics : Microprocessors : RS485 communications

위 링크에서 내리다 보면 이렇게 라이브러리를 다운로드할 수 있는 링크가 나온다. 이를 아두이노 라이브러리에 추가해주기 바란다.
그러기 위해서 아두이노를 실행하자!

 

arduino

※ 코드 작성

 

#include <ros.h> 
#include "rs485/num.h" 
#include <RS485_protocol.h> 
#include <SoftwareSerial.h>

ros::NodeHandle nh; 
using rs485::num;
SoftwareSerial RS485(2, 3);

uint8_t header1 = 255; 
uint8_t header2 = 254; 
uint8_t id = 0; 
uint8_t dataSize = 7; 
uint8_t mode = 1;
uint8_t _direction = 0;
uint8_t position1 = 70;
uint8_t position2 = 80; 
uint8_t velocity1 = 0; 
uint8_t velocity2 = 50;
uint8_t checkSum = ~(id + dataSize + mode + _direction + position1 + position2 + velocity1 + velocity2); 

const int bufferSize = 11;
uint8_t dataArray[bufferSize] = {header1, header2, id, dataSize, checkSum, mode, _direction, position1, position2, velocity1, velocity2}; 

void serviceInfo(const num::Request &req, num::Response &res) {
	res.result = dataArray[req.x]; 
}

ros::ServiceServer<num::Request, num::Response> server("rs485", &serviceInfo); 

void setup() { 
	nh.initNode(); 
	nh.advertiseService(server);
	RS485.begin(9600);
	pinMode(dataPin, OUTPUT);
} 

void loop() {
	//delay(1000);
	byte dataArray[bufferSize] = {header1, header2, id, dataSize, checkSum, mode, _direction, position1, position2, velocity1, velocity2}; 
	for (int i = 0; i < sizeof dataArray; i++) { 
		RS485.write(dataArray[i]); 
    	if (i == 10) { delay(1); }
    } 
    nh.spinOnce(); 
}

작은 숫자로만 변수들을 표현할 것이기 때문에 unsigned char가 아닌 uin8_t로 표현하였다. uint8_t로 선언한 변수들은 누리로봇 액츄에이터에 보낼 정보들이다. 기기의 헤더 1,2와 id, 시계 or 반시계, 속도 등등을 16진수로 표현하여 전송해야한다. void loop()에서는 byte로 재선언하여 이를 RS485 컨버터에 전달한다. [byte 형식으로 통신하면 문제없이 작동한다.]

누리로봇 위치, 속도 제어

그리고 여기까지 진행이 됐다면 아두이노는 #include "rs485/num.h" 애가 누구니? 라고 계속 물어보고 있을 것이다.

 

rosrun rosserial_arduino make_libraries.py ~/Arduino/libraries/

 

 

 

위 명령을 터미널에 입력하자 roscore를 실행하지 않아도 괜찮다. 아두이노가 인식할 수 있도록 ros의 라이브러리도 생성하지만 이 사이에는 메시지 파일의 전송도 담겨있다. 만약에 메시지 내용을 수정한다면 위 작업을 다시 해주어야한다. 위에 있는 rosrun make_libraries 명령어를 .bashrc 파일에 alias를 이용하여 간단한 몇 단어로 저장해놓으면 이후에 메시지, 서비스 형식을 사용할때 편리할 것이다.

 

혹시 이미 ros_lib가 존재한다는 문구가 뜬다면 ros_lib를 삭제해준 후에 다시 실행해주기 바란다. 지우지 않으면 이미 존재하는 디렉토리라는 에러가 발생한다.

 

sudo rm -rf ~/Arduino/libraries/ros_lib

※ retured None 에러 해결 방법

 

// 터미널 1 
roscore 

// 터미널 2 
rosrun rosserial_arduino serial_node.py /dev/ttyACM0 

// 터미널 3 
rosrun rs485 client

 

2번째 터미널에서 /dev/ttyACM0은 아두이노 장치를 의미한다. 만약 해당 포트에 권한이 없으면 읽고 쓰기 권한을 부여해야한다.

 

 

sudo chmod a+rw /dev/ttyACM0

 

해당 포트의 읽고 쓰기 권한을 모든 사용자에게 주는 명령어이다. 사실 모든 사용자에게 주는 것은 보안상 위험할 수 있으나 집에서 개인적으로 사용하기 때문에 상관이 없다고 생각하고 넘어갔다.

그런데 요렇게 하고 그냥 실행하면 서비스 통신이 제대로 되지 않는다!

"ERROR: service [/topic] responded with an error: service cannot process request: service handler returned None"

 

분명 제대로 해줬는데 서비스 핸들러가 None을 반환했다고만 에러가 나온다! 다행히도 해결하는 방법이 있다. 아래 링크에 해결책이 있다. 해당 솔루션을 따라 순서대로 해보자.

rosserial_python: handle service responses correctly by lenzc · Pull Request #414 · ros-drivers/rosserial

 

먼저 rosserial_python 디렉토리로 이동한다.

cd /opt/ros/melodic/lib/python2.7/dist-packages/rosserial_python

그리고 나서 원래 있던 파일 SerialClient.py와 SerialClient.pyc의 내용을 변경하면서도 기존 내용을 보존하기위해 이름을 바꾸어놓자. 수정할 파일은 SerialClient.py이다. 깃허브에서 .pyc 파일은 따로 언급하지 않아서 넘어간다.

 

sudo mv SerialClient.pyc melodicSerialClient.pyc sudo mv SerialClient.py melodicSerialClient.py

완료했으면 편집기를 이용하여 SerialClient.py와 SerialClient.pyc를 만들어 안에 내용을 붙여주자.

 

더보기를 누르시면 내용을 볼 수 있습니다.

SerialClient.py 파일 내용입니다! 내용을 확인하고 붙여넣기 해주세요.

 

#!/usr/bin/env python

#####################################################################
# Software License Agreement (BSD License)
#
# Copyright (c) 2011, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of Willow Garage, Inc. nor the names of its
#    contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

__author__ = "mferguson@willowgarage.com (Michael Ferguson)"

import roslib
import rospy
import imp

import thread
import multiprocessing
from serial import *
import StringIO

from std_msgs.msg import Time
from rosserial_msgs.msg import *
from rosserial_msgs.srv import *

import diagnostic_msgs.msg

import errno
import signal
import socket
import struct
import time


def load_pkg_module(package, directory):
    #check if its in the python path
    path = sys.path
    try:
        imp.find_module(package)
    except:
        roslib.load_manifest(package)
    try:
        m = __import__( package + '.' + directory )
    except:
        rospy.logerr( "Cannot import package : %s"% package )
        rospy.logerr( "sys.path was " + str(path) )
        return None
    return m

def load_message(package, message):
    m = load_pkg_module(package, 'msg')
    m2 = getattr(m, 'msg')
    return getattr(m2, message)

def load_service(package,service):
    s = load_pkg_module(package, 'srv')
    s = getattr(s, 'srv')
    srv = getattr(s, service)
    mreq = getattr(s, service+"Request")
    mres = getattr(s, service+"Response")
    return srv,mreq,mres

class Publisher:
    """
        Publisher forwards messages from the serial device to ROS.
    """
    def __init__(self, topic_info):
        """ Create a new publisher. """
        self.topic = topic_info.topic_name

        # find message type
        package, message = topic_info.message_type.split('/')
        self.message = load_message(package, message)
        if self.message._md5sum == topic_info.md5sum:
            self.publisher = rospy.Publisher(self.topic, self.message, queue_size=10)
        else:
            raise Exception('Checksum does not match: ' + self.message._md5sum + ',' + topic_info.md5sum)

    def handlePacket(self, data):
        """ Forward message to ROS network. """
        m = self.message()
        m.deserialize(data)
        self.publisher.publish(m)


class Subscriber:
    """
        Subscriber forwards messages from ROS to the serial device.
    """

    def __init__(self, topic_info, parent):
        self.topic = topic_info.topic_name
        self.id = topic_info.topic_id
        self.parent = parent

        # find message type
        package, message = topic_info.message_type.split('/')
        self.message = load_message(package, message)
        if self.message._md5sum == topic_info.md5sum:
            self.subscriber = rospy.Subscriber(self.topic, self.message, self.callback)
        else:
            raise Exception('Checksum does not match: ' + self.message._md5sum + ',' + topic_info.md5sum)

    def unregister(self):
        rospy.loginfo("Removing subscriber: %s", self.topic)
        self.subscriber.unregister()

    def callback(self, msg):
        """ Forward message to serial device. """
        data_buffer = StringIO.StringIO()
        msg.serialize(data_buffer)
        self.parent.send(self.id, data_buffer.getvalue())

    def unregister(self):
        self.subscriber.unregister()

class ServiceServer:
    """
        ServiceServer responds to requests from ROS.
    """

    def __init__(self, topic_info, parent):
        self.topic = topic_info.topic_name
        self.parent = parent

        # find message type
        package, service = topic_info.message_type.split('/')
        s = load_pkg_module(package, 'srv')
        s = getattr(s, 'srv')
        self.mreq = getattr(s, service+"Request")
        self.mres = getattr(s, service+"Response")
        srv = getattr(s, service)
        self.service = rospy.Service(self.topic, srv, self.callback)

        # response message
        self.data = None

    def unregister(self):
        rospy.loginfo("Removing service: %s", self.topic)
        self.service.shutdown()

    def callback(self, req):
        """ Forward request to serial device. """
        data_buffer = StringIO.StringIO()
        req.serialize(data_buffer)
        self.response = None
        if self.parent.send(self.id, data_buffer.getvalue()) >= 0:
            while self.response == None:
                pass
        return self.response

    def handlePacket(self, data):
        """ Forward response to ROS network. """
        r = self.mres()
        r.deserialize(data)
        self.response = r


class ServiceClient:
    """
        ServiceServer responds to requests from ROS.
    """

    def __init__(self, topic_info, parent):
        self.topic = topic_info.topic_name
        self.parent = parent

        # find message type
        package, service = topic_info.message_type.split('/')
        s = load_pkg_module(package, 'srv')
        s = getattr(s, 'srv')
        self.mreq = getattr(s, service+"Request")
        self.mres = getattr(s, service+"Response")
        srv = getattr(s, service)
        rospy.loginfo("Starting service client, waiting for service '" + self.topic + "'")
        rospy.wait_for_service(self.topic)
        self.proxy = rospy.ServiceProxy(self.topic, srv)

    def handlePacket(self, data):
        """ Forward request to ROS network. """
        req = self.mreq()
        req.deserialize(data)
        # call service proxy
        resp = self.proxy(req)
        # serialize and publish
        data_buffer = StringIO.StringIO()
        resp.serialize(data_buffer)
        self.parent.send(self.id, data_buffer.getvalue())

class RosSerialServer:
    """
        RosSerialServer waits for a socket connection then passes itself, forked as a
        new process, to SerialClient which uses it as a serial port. It continues to listen
        for additional connections. Each forked process is a new ros node, and proxies ros
        operations (e.g. publish/subscribe) from its connection to the rest of ros.
    """
    def __init__(self, tcp_portnum, fork_server=False):
        print "Fork_server is: ", fork_server
        self.tcp_portnum = tcp_portnum
        self.fork_server = fork_server

    def listen(self):
        self.serversocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.serversocket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        #bind the socket to a public host, and a well-known port
        self.serversocket.bind(("", self.tcp_portnum)) #become a server socket
        self.serversocket.listen(1)

        while True:
            #accept connections
            print "waiting for socket connection"
            (clientsocket, address) = self.serversocket.accept()

            #now do something with the clientsocket
            rospy.loginfo("Established a socket connection from %s on port %s" % (address))
            self.socket = clientsocket
            self.isConnected = True

            if (self.fork_server == True):	# if configured to launch server in a separate process
                rospy.loginfo("Forking a socket server process")
                process = multiprocessing.Process(target=self.startSocketServer, args=(address))
                process.daemon = True
                process.start()
                rospy.loginfo("launched startSocketServer")
            else:
                rospy.loginfo("calling startSerialClient")
                self.startSerialClient()
                rospy.loginfo("startSerialClient() exited")

    def startSerialClient(self):
        client = SerialClient(self)
        try:
            client.run()
        except KeyboardInterrupt:
            pass
        except RuntimeError:
            rospy.loginfo("RuntimeError exception caught")
            self.isConnected = False
        except socket.error:
            rospy.loginfo("socket.error exception caught")
            self.isConnected = False
        finally:
            self.socket.close()
            for sub in client.subscribers.values():
                sub.unregister()
            for srv in client.services.values():
                srv.unregister()
            #pass

    def startSocketServer(self, port, address):
        rospy.loginfo("starting ROS Serial Python Node serial_node-%r" % (address,))
        rospy.init_node("serial_node_%r" % (address,))
        self.startSerialClient()

    def flushInput(self):
         pass

    def write(self, data):
        if (self.isConnected == False):
            return
        length = len(data)
        totalsent = 0

        while totalsent < length:
            sent = self.socket.send(data[totalsent:])
            if sent == 0:
                raise RuntimeError("RosSerialServer.write() socket connection broken")
            totalsent = totalsent + sent

    def read(self, rqsted_length):
        self.msg = ''
        if (self.isConnected == False):
            return self.msg

        while len(self.msg) < rqsted_length:
            chunk = self.socket.recv(rqsted_length - len(self.msg))
            if chunk == '':
                raise RuntimeError("RosSerialServer.read() socket connection broken")
            self.msg = self.msg + chunk
        return self.msg

    def inWaiting(self):
        try: # the caller checks just for <1, so we'll peek at just one byte
            chunk = self.socket.recv(1, socket.MSG_DONTWAIT|socket.MSG_PEEK)
            if chunk == '':
                raise RuntimeError("RosSerialServer.inWaiting() socket connection broken")
            return len(chunk)
        except socket.error, e:
            if e.args[0] == errno.EWOULDBLOCK:
                return 0
            raise


class SerialClient:
    """
        ServiceServer responds to requests from the serial device.
    """

    def __init__(self, port=None, baud=57600, timeout=5.0, fix_pyserial_for_test=False):
        """ Initialize node, connect to bus, attempt to negotiate topics. """
        self.mutex = thread.allocate_lock()

        self.lastsync = rospy.Time(0)
        self.lastsync_lost = rospy.Time(0)
        self.timeout = timeout
        self.synced = False
        self.fix_pyserial_for_test = fix_pyserial_for_test

        self.pub_diagnostics = rospy.Publisher('/diagnostics', diagnostic_msgs.msg.DiagnosticArray, queue_size=10)

        if port== None:
            # no port specified, listen for any new port?
            pass
        elif hasattr(port, 'read'):
            #assume its a filelike object
            self.port=port
        else:
            # open a specific port
            while not rospy.is_shutdown():
                try:
                    if self.fix_pyserial_for_test:
                        # see https://github.com/pyserial/pyserial/issues/59
                        self.port = Serial(port, baud, timeout=self.timeout*0.5, rtscts=True, dsrdtr=True)
                    else:
                        self.port = Serial(port, baud, timeout=self.timeout*0.5)
                    break
                except SerialException as e:
                    rospy.logerr("Error opening serial: %s", e)
                    time.sleep(3)

        if rospy.is_shutdown():
            return

        self.port.timeout = 0.01  # Edit the port timeout

        time.sleep(0.1)           # Wait for ready (patch for Uno)

        # hydro introduces protocol ver2 which must match node_handle.h
        # The protocol version is sent as the 2nd sync byte emitted by each end
        self.protocol_ver1 = '\xff'
        self.protocol_ver2 = '\xfe'
        self.protocol_ver = self.protocol_ver2

        self.publishers = dict()  # id:Publishers
        self.subscribers = dict() # topic:Subscriber
        self.services = dict()    # topic:Service

        self.buffer_out = -1
        self.buffer_in = -1

        self.callbacks = dict()
        # endpoints for creating new pubs/subs
        self.callbacks[TopicInfo.ID_PUBLISHER] = self.setupPublisher
        self.callbacks[TopicInfo.ID_SUBSCRIBER] = self.setupSubscriber
        # service client/servers have 2 creation endpoints (a publisher and a subscriber)
        self.callbacks[TopicInfo.ID_SERVICE_SERVER+TopicInfo.ID_PUBLISHER] = self.setupServiceServerPublisher
        self.callbacks[TopicInfo.ID_SERVICE_SERVER+TopicInfo.ID_SUBSCRIBER] = self.setupServiceServerSubscriber
        self.callbacks[TopicInfo.ID_SERVICE_CLIENT+TopicInfo.ID_PUBLISHER] = self.setupServiceClientPublisher
        self.callbacks[TopicInfo.ID_SERVICE_CLIENT+TopicInfo.ID_SUBSCRIBER] = self.setupServiceClientSubscriber
        # custom endpoints
        self.callbacks[TopicInfo.ID_PARAMETER_REQUEST] = self.handleParameterRequest
        self.callbacks[TopicInfo.ID_LOG] = self.handleLoggingRequest
        self.callbacks[TopicInfo.ID_TIME] = self.handleTimeRequest

        rospy.sleep(2.0) # TODO
        self.requestTopics()
        self.lastsync = rospy.Time.now()

        signal.signal(signal.SIGINT, self.txStopRequest)

    def requestTopics(self):
        """ Determine topics to subscribe/publish. """
        # TODO remove if possible
        if not self.fix_pyserial_for_test:
            self.port.flushInput()

        # request topic sync
        self.port.write("\xff" + self.protocol_ver + "\x00\x00\xff\x00\x00\xff")

    def txStopRequest(self, signal, frame):
        """ send stop tx request to arduino when receive SIGINT(Ctrl-c)"""
        if not self.fix_pyserial_for_test:
            self.port.flushInput()

        self.port.write("\xff" + self.protocol_ver + "\x00\x00\xff\x0b\x00\xf4")
        # tx_stop_request is x0b
        rospy.loginfo("Send tx stop request")
        sys.exit(0)

    def tryRead(self, length):
        try:
            read_start = time.time()
            read_current = read_start
            bytes_remaining = length
            result = bytearray()
            while bytes_remaining != 0 and read_current - read_start < self.timeout:
                received = self.port.read(bytes_remaining)
                if len(received) != 0:
                    result.extend(received)
                    bytes_remaining -= len(received)
                read_current = time.time()

            if bytes_remaining != 0:
                rospy.logwarn("Serial Port read returned short (expected %d bytes, received %d instead)."
                              % (length, length - bytes_remaining))
                raise IOError()

            return bytes(result)
        except Exception as e:
            rospy.logwarn("Serial Port read failure: %s", e)
            raise IOError()

    def run(self):
        """ Forward recieved messages to appropriate publisher. """
        data = ''
        while not rospy.is_shutdown():
            if (rospy.Time.now() - self.lastsync).to_sec() > (self.timeout * 3):
                if (self.synced == True):
                    rospy.logerr("Lost sync with device, restarting...")
                else:
                    rospy.logerr("Unable to sync with device; possible link problem or link software version mismatch such as hydro rosserial_python with groovy Arduino")
                self.lastsync_lost = rospy.Time.now()
                self.sendDiagnostics(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "no sync with device")
                self.requestTopics()
                self.lastsync = rospy.Time.now()

            # This try-block is here because we make multiple calls to read(). Any one of them can throw
            # an IOError if there's a serial problem or timeout. In that scenario, a single handler at the
            # bottom attempts to reconfigure the topics.
            try:
                if self.port.inWaiting() < 1:
                    time.sleep(0.001)
                    continue

                flag = [0,0]
                flag[0] = self.tryRead(1)
                if (flag[0] != '\xff'):
                    continue

                flag[1] = self.tryRead(1)
                if ( flag[1] != self.protocol_ver):
                    self.sendDiagnostics(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Mismatched protocol version in packet: lost sync or rosserial_python is from different ros release than the rosserial client")
                    rospy.logerr("Mismatched protocol version in packet: lost sync or rosserial_python is from different ros release than the rosserial client")
                    protocol_ver_msgs = {'\xff': 'Rev 0 (rosserial 0.4 and earlier)', '\xfe': 'Rev 1 (rosserial 0.5+)', '\xfd': 'Some future rosserial version'}
                    if (flag[1] in protocol_ver_msgs):
                        found_ver_msg = 'Protocol version of client is ' + protocol_ver_msgs[flag[1]]
                    else:
                        found_ver_msg = "Protocol version of client is unrecognized"
                    rospy.loginfo("%s, expected %s" % (found_ver_msg, protocol_ver_msgs[self.protocol_ver]))
                    continue

                msg_len_bytes = self.tryRead(2)
                msg_length, = struct.unpack("<h", msg_len_bytes)

                msg_len_chk = self.tryRead(1)
                msg_len_checksum = sum(map(ord, msg_len_bytes)) + ord(msg_len_chk)

                if msg_len_checksum % 256 != 255:
                    rospy.loginfo("wrong checksum for msg length, length %d" %(msg_length))
                    rospy.loginfo("chk is %d" % ord(msg_len_chk))
                    continue

                # topic id (2 bytes)
                topic_id_header = self.tryRead(2)
                topic_id, = struct.unpack("<h", topic_id_header)

                try:
                    msg = self.tryRead(msg_length)
                except IOError:
                    self.sendDiagnostics(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Packet Failed : Failed to read msg data")
                    rospy.loginfo("Packet Failed :  Failed to read msg data")
                    rospy.loginfo("expected msg length is %d", msg_length)
                    raise

                # checksum for topic id and msg
                chk = self.tryRead(1)
                checksum = sum(map(ord, topic_id_header) ) + sum(map(ord, msg)) + ord(chk)

                if checksum % 256 == 255:
                    self.synced = True
                    try:
                        self.callbacks[topic_id](msg)
                    except KeyError:
                        rospy.logerr("Tried to publish before configured, topic id %d" % topic_id)
                        self.requestTopics()
                    rospy.sleep(0.001)
                else:
                    rospy.loginfo("wrong checksum for topic id and msg")

            except IOError:
                # One of the read calls had an issue. Just to be safe, request that the client
                # reinitialize their topics.
                self.requestTopics()

    def setPublishSize(self, bytes):
        if self.buffer_out < 0:
            self.buffer_out = bytes
            rospy.loginfo("Note: publish buffer size is %d bytes" % self.buffer_out)

    def setSubscribeSize(self, bytes):
        if self.buffer_in < 0:
            self.buffer_in = bytes
            rospy.loginfo("Note: subscribe buffer size is %d bytes" % self.buffer_in)

    def setupPublisher(self, data):
        """ Register a new publisher. """
        try:
            msg = TopicInfo()
            msg.deserialize(data)
            pub = Publisher(msg)
            self.publishers[msg.topic_id] = pub
            self.callbacks[msg.topic_id] = pub.handlePacket
            self.setPublishSize(msg.buffer_size)
            rospy.loginfo("Setup publisher on %s [%s]" % (msg.topic_name, msg.message_type) )
        except Exception as e:
            rospy.logerr("Creation of publisher failed: %s", e)

    def setupSubscriber(self, data):
        """ Register a new subscriber. """
        try:
            msg = TopicInfo()
            msg.deserialize(data)
            if not msg.topic_name in self.subscribers.keys():
                sub = Subscriber(msg, self)
                self.subscribers[msg.topic_name] = sub
                self.setSubscribeSize(msg.buffer_size)
                rospy.loginfo("Setup subscriber on %s [%s]" % (msg.topic_name, msg.message_type) )
            elif msg.message_type != self.subscribers[msg.topic_name].message._type:
                old_message_type = self.subscribers[msg.topic_name].message._type
                self.subscribers[msg.topic_name].unregister()
                sub = Subscriber(msg, self)
                self.subscribers[msg.topic_name] = sub
                self.setSubscribeSize(msg.buffer_size)
                rospy.loginfo("Change the message type of subscriber on %s from [%s] to [%s]" % (msg.topic_name, old_message_type, msg.message_type) )
        except Exception as e:
            rospy.logerr("Creation of subscriber failed: %s", e)

    def setupServiceServerPublisher(self, data):
        """ Register a new service server. """
        try:
            msg = TopicInfo()
            msg.deserialize(data)
            self.setPublishSize(msg.buffer_size)
            try:
                srv = self.services[msg.topic_name]
            except:
                srv = ServiceServer(msg, self)
                rospy.loginfo("Setup service server on %s [%s]" % (msg.topic_name, msg.message_type) )
                self.services[msg.topic_name] = srv
            if srv.mres._md5sum == msg.md5sum:
                self.callbacks[msg.topic_id] = srv.handlePacket
            else:
                raise Exception('Checksum does not match: ' + srv.mres._md5sum + ',' + msg.md5sum)
        except Exception as e:
            rospy.logerr("Creation of service server failed: %s", e)
    def setupServiceServerSubscriber(self, data):
        """ Register a new service server. """
        try:
            msg = TopicInfo()
            msg.deserialize(data)
            self.setSubscribeSize(msg.buffer_size)
            try:
                srv = self.services[msg.topic_name]
            except:
                srv = ServiceServer(msg, self)
                rospy.loginfo("Setup service server on %s [%s]" % (msg.topic_name, msg.message_type) )
                self.services[msg.topic_name] = srv
            if srv.mreq._md5sum == msg.md5sum:
                srv.id = msg.topic_id
            else:
                raise Exception('Checksum does not match: ' + srv.mreq._md5sum + ',' + msg.md5sum)
        except Exception as e:
            rospy.logerr("Creation of service server failed: %s", e)

    def setupServiceClientPublisher(self, data):
        """ Register a new service client. """
        try:
            msg = TopicInfo()
            msg.deserialize(data)
            self.setPublishSize(msg.buffer_size)
            try:
                srv = self.services[msg.topic_name]
            except:
                srv = ServiceClient(msg, self)
                rospy.loginfo("Setup service client on %s [%s]" % (msg.topic_name, msg.message_type) )
                self.services[msg.topic_name] = srv
            if srv.mreq._md5sum == msg.md5sum:
                self.callbacks[msg.topic_id] = srv.handlePacket
            else:
                raise Exception('Checksum does not match: ' + srv.mreq._md5sum + ',' + msg.md5sum)
        except Exception as e:
            rospy.logerr("Creation of service client failed: %s", e)
    def setupServiceClientSubscriber(self, data):
        """ Register a new service client. """
        try:
            msg = TopicInfo()
            msg.deserialize(data)
            self.setSubscribeSize(msg.buffer_size)
            try:
                srv = self.services[msg.topic_name]
            except:
                srv = ServiceClient(msg, self)
                rospy.loginfo("Setup service client on %s [%s]" % (msg.topic_name, msg.message_type) )
                self.services[msg.topic_name] = srv
            if srv.mres._md5sum == msg.md5sum:
                srv.id = msg.topic_id
            else:
                raise Exception('Checksum does not match: ' + srv.mres._md5sum + ',' + msg.md5sum)
        except Exception as e:
            rospy.logerr("Creation of service client failed: %s", e)

    def handleTimeRequest(self, data):
        """ Respond to device with system time. """
        t = Time()
        t.data = rospy.Time.now()
        data_buffer = StringIO.StringIO()
        t.serialize(data_buffer)
        self.send( TopicInfo.ID_TIME, data_buffer.getvalue() )
        self.lastsync = rospy.Time.now()


    def handleParameterRequest(self, data):
        """ Send parameters to device. Supports only simple datatypes and arrays of such. """
        req = RequestParamRequest()
        req.deserialize(data)
        resp = RequestParamResponse()
        try:
            param = rospy.get_param(req.name)
        except KeyError:
            rospy.logerr("Parameter %s does not exist"%req.name)
            return

        if param == None:
            rospy.logerr("Parameter %s does not exist"%req.name)
            return

        if (type(param) == dict):
            rospy.logerr("Cannot send param %s because it is a dictionary"%req.name)
            return
        if (type(param) != list):
            param = [param]
        #check to make sure that all parameters in list are same type
        t = type(param[0])
        for p in param:
            if t!= type(p):
                rospy.logerr('All Paramers in the list %s must be of the same type'%req.name)
                return
        if (t == int or t == bool):
            resp.ints= param
        if (t == float):
            resp.floats=param
        if (t == str):
            resp.strings = param
        data_buffer = StringIO.StringIO()
        resp.serialize(data_buffer)
        self.send(TopicInfo.ID_PARAMETER_REQUEST, data_buffer.getvalue())

    def handleLoggingRequest(self, data):
        """ Forward logging information from serial device into ROS. """
        msg = Log()
        msg.deserialize(data)
        if (msg.level == Log.ROSDEBUG):
            rospy.logdebug(msg.msg)
        elif(msg.level== Log.INFO):
            rospy.loginfo(msg.msg)
        elif(msg.level== Log.WARN):
            rospy.logwarn(msg.msg)
        elif(msg.level== Log.ERROR):
            rospy.logerr(msg.msg)
        elif(msg.level==Log.FATAL):
            rospy.logfatal(msg.msg)

    def send(self, topic, msg):
        """ Send a message on a particular topic to the device. """
        with self.mutex:
            length = len(msg)
            if self.buffer_in > 0 and length > self.buffer_in:
                rospy.logerr("Message from ROS network dropped: message larger than buffer.")
                print msg
                return -1
            else:
                    #modified frame : header(2 bytes) + msg_len(2 bytes) + msg_len_chk(1 byte) + topic_id(2 bytes) + msg(x bytes) + msg_topic_id_chk(1 byte)
                    # second byte of header is protocol version
                    msg_len_checksum = 255 - ( ((length&255) + (length>>8))%256 )
                    msg_checksum = 255 - ( ((topic&255) + (topic>>8) + sum([ord(x) for x in msg]))%256 )
                    data = "\xff" + self.protocol_ver  + chr(length&255) + chr(length>>8) + chr(msg_len_checksum) + chr(topic&255) + chr(topic>>8)
                    data = data + msg + chr(msg_checksum)
                    self.port.write(data)
                    return length

    def sendDiagnostics(self, level, msg_text):
        msg = diagnostic_msgs.msg.DiagnosticArray()
        status = diagnostic_msgs.msg.DiagnosticStatus()
        status.name = "rosserial_python"
        msg.header.stamp = rospy.Time.now()
        msg.status.append(status)

        status.message = msg_text
        status.level = level

        status.values.append(diagnostic_msgs.msg.KeyValue())
        status.values[0].key="last sync"
        if self.lastsync.to_sec()>0:
            status.values[0].value=time.ctime(self.lastsync.to_sec())
        else:
            status.values[0].value="never"

        status.values.append(diagnostic_msgs.msg.KeyValue())
        status.values[1].key="last sync lost"
        status.values[1].value=time.ctime(self.lastsync_lost.to_sec())

        self.pub_diagnostics.publish(msg)

 

위 작업을 완료했으면 문제 없이 서비스 콜을 할 수 있을 것이다!

 

---

③ 실행 결과

 

 

문제 없이 작동한다. RS485 컨버터가 아두이노에서 설정한 delay 주기에 맞춰 깜빡깜빡 통신을 한다. 누리로봇도 힘이 아주아주 강력한 녀석이라 로봇 팔 두 세번째 관절에 사용할 수 있다. 물론 가격이 좀 있기는 하지만 고토크에 엔코더 기능이 달려있어 "정지 동작 후 수행"만 즉각 즉각 전달이 된다면 팔에 대한 부담을 덜 수 있는 모델이 아닐까 싶다.

아직 해보지는 않았지만 여러가지 글들을 찾아보면서 다중 채널의 USB 허브에 아두이노를 슬레이브로써 사용할 수 있다고 한다. 아두이노에 미리 코드를 업로드하고 Serial_node.py를 여러개 만들어(이름도 여러개로...) 실행하면 제대로 작동하는 사례를 보았다. 나중에 기회가 되면 다중으로 아두이노를 사용해서 통신하는 구조를 만들어봐야겠다.