arduino设备跑 ros service server 的波折记

最新推荐文章于 2023-07-12 12:48:24 发布

qq_278667286

最新推荐文章于 2023-07-12 12:48:24 发布

阅读量1.7k

点赞数 6

分类专栏： ros arduino

本文链接：https://blog.youkuaiyun.com/qq_38288618/article/details/104338668

版权

ros 同时被 2 个专栏收录

64 篇文章

订阅专栏

arduino

41 篇文章

订阅专栏

本文记录了在Arduino设备上实现ROS Service Server过程中的波折与解决方法，包括库版本冲突、通信错误及返回值问题的排查与修复。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

arduino设备跑 ros service server 的波折记。

引——

参考前文https://blog.youkuaiyun.com/qq_38288618/article/details/104082877
创建工作空间、相关包、srv、makelist，catkinmake完成，调用
rosrun rosserial_arduino make_libraries.py 生成arduino的 lib，复制相关lib到arduino库，
写个简单的serviceserver示例，编译下载成功。这一系列动作之后rosrun个节点来验证通信。

波——

运行命令遇到错误

PS D:\project\ros\zzz_srv_ws> rosrun rosserial_python serial_node.py COM8
[INFO] [1581539992.670000]: ROS Serial Python Node
[INFO] [1581539992.686000]: zzzConnecting to COM8 at 57600 baud
[INFO] [1581539994.799000]: Requesting topics...
[INFO] [1581539994.840000]: Note: publish buffer size is 280 bytes
[ERROR] [1581539994.971000]: Creation of service server failed: rosserial_arduino
ROS path [0]=C:\opt\ros\melodic\x64\share\ros
ROS path [1]=C:\opt\ros\melodic\x64\share
ROS path [2]=D:/project/ros/zzz_srv_ws/src
[INFO] [1581539994.977000]: Setup publisher on chatter [std_msgs/String]
[INFO] [1581539994.987000]: Note: subscribe buffer size is 280 bytes
[ERROR] [1581539994.993000]: Creation of service server failed: rosserial_arduino
ROS path [0]=C:\opt\ros\melodic\x64\share\ros
ROS path [1]=C:\opt\ros\melodic\x64\share
ROS path [2]=D:/project/ros/zzz_srv_ws/src

顺藤摸瓜，搜索 Creation of service server failed: 相关文件

SerialClient.py

def setupServiceServerPublisher(self, data):
        """ Register a new service server. """
        try:
            msg = TopicInfo()
            msg.deserialize(data)
            rospy.loginfo("zzz data=%s"%data)
            self.setPublishSize(msg.buffer_size)
            rospy.loginfo("zzz msg.buffer_size=%s"%msg.buffer_size)
            try:
                rospy.loginfo("zzz:msgname[type]= %s [%s]" % (msg.topic_name, msg.message_type) )
                srv = self.services[msg.topic_name]
                rospy.loginfo("try")
            except KeyError:
                srv = ServiceServer(msg, self)
                rospy.loginfo("key error")
                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)

print大法得到

[INFO] [1581576166.920000]: ROS Serial Python Node
[INFO] [1581576166.939000]: zzzConnecting to COM8 at 57600 baud
[INFO] [1581576169.046000]: Requesting topics...
[INFO] [1581576169.080000]: zzz data=   test_srv   rosserial_arduino/Test    0825d95fdfa2c8f4bbb4e9c74bccd3fd
[INFO] [1581576169.111000]: Note: publish buffer size is 280 bytes
[INFO] [1581576169.117000]: zzz msg.buffer_size=280
[INFO] [1581576169.121000]: zzz:msgname[type]= test_srv [rosserial_arduino/Test]
[ERROR] [1581576169.248000]: Creation of service server failed: rosserial_arduino
ROS path [0]=C:\opt\ros\melodic\x64\share\ros
ROS path [1]=C:\opt\ros\melodic\x64\share
ROS path [2]=D:/project/ros/zzz_srv_ws/src
[INFO] [1581576169.253000]: Setup publisher on chatter [std_msgs/String]

找不到问题，反复调试，用arduino示例能跑，新生成的就不行，调到怀疑人生。。。。。
最后一气之下把arduino的ros库盖了一把，发现没毛病，竟然好了。！！！？？？！！！。。。。

分析
早些时候安装了arduino的ros库，这次只把相关的头文件考过来了，有可能版本不一致，导致这个问题。
用新的lib把arduino的ros库给覆盖一下就ok了。我的疏忽，浪费了生命，大家引以为戒吧。

此时松了一口气，serviceserver最终ok。输出如下

[INFO] [1581784397.551000]: Note: publish buffer size is 280 bytes
[INFO] [1581784397.563000]: zzz msg.buffer_size=280
[INFO] [1581784397.570000]: zzz:msgname[type]= zzz_srv [serialhwinfo/hwinfo]
[INFO] [1581784397.622000]: key error
[INFO] [1581784397.625000]: Setup service server on zzz_srv [serialhwinfo/hwinfo]

折——

但、但、但、但发现通信还是问题：

responded with an error: service cannot process request: service handler returned None

再深呼吸，调整下自己的位姿，依旧使用顺藤摸瓜大法：

找到文件
C:\opt\ros\melodic\x64\lib\site-packages\rospy\impl
tcpros_service.py就是这个函数输出，当response == None:时是这个错误

def convert_return_to_response(response, response_class):
    """
    Convert return value of function to response instance. The
    rules/precedence for this are:

    1. If the return type is the same as the response type, no conversion
    is done.

    2. If the return type is a dictionary, it is used as a keyword-style
    initialization for a new response instance.

    3. If the return type is *not* a list type, it is passed in as a single arg
    to a new response instance.

    4. If the return type is a list/tuple type, it is used as a args-style
    initialization for a new response instance.
    """

    # use this declared ROS type check instead of a direct instance
    # check, which allows us to play tricks with serialization and
    # deserialization
    if isinstance(response, genpy.Message) and response._type == response_class._type:
    #if isinstance(response, response_class):
        return response
    elif type(response) == dict:
        # kwds response
        try:
            return response_class(**response)
        except AttributeError as e:
            raise ServiceException("handler returned invalid value: %s"%str(e))
    elif response == None:
        raise ServiceException("service handler returned None")
    elif type(response) not in [list, tuple]:
        # single, non-list arg
        try:
            return response_class(response)
        except TypeError as e:
            raise ServiceException("handler returned invalid value: %s"%str(e))
    else:
        # user returned a list, which has some ambiguous cases. Our resolution is that
        # all list/tuples are converted to *args
        try:
            return response_class(*response)
        except TypeError as e:
            raise ServiceException("handler returned wrong number of values: %s"%str(e))

这里调用了convert_return_to_response，让我们来加些print

def _handle_request(self, transport, request):
        """
        Process a single incoming request.
        @param transport: transport instance
        @type  transport: L{TCPROSTransport}
        @param request: Message
        @type  request: genpy.Message
        """
        try:
            print("zzz 20200215")
            # convert return type to response Message instance
            print(self.response_class)
            print(self.handler)
            print(request)
            print("handler=",self.handler(request))
            response = convert_return_to_response(self.handler(request), self.response_class)
            self.seq += 1
            print("seq=",self.seq)
            print(transport.write_buff)
            # ok byte
            transport.write_buff.write(struct.pack('<B', 1))
            
            transport.send_message(response, self.seq)
        except ServiceException as e:
            rospy.core.rospydebug("handler raised ServiceException: %s"%(e))
            self._write_service_error(transport, "service cannot process request: %s"%e)
        except Exception as e:
            (exc_type, exc_value, tb) = sys.exc_info()
            self.error_handler(e, exc_type, exc_value, tb)
            self._write_service_error(transport, "error processing request: %s"%e)

底部注释

If handler cannot process request, it may either return None,
to indicate failure, or it may raise a rospy.ServiceException
to send a specific error message to the client. Returning None
is always considered a failure.
简译：
若handler无法处理请求，它可能返回None以表示失败，
也可能是引起了rospy.ServiceException错误，来向客户端发送特定的错误消息。返回None总被认为是失败的。。。。

对这个说明不是很满意。

看看输出捋捋吧

zzz 20200215
<class 'serialhwinfo.srv._hwinfo.hwinfoResponse'>
<bound method ServiceServer.callback of <rosserial_python.SerialClient.ServiceServer instance at 0x000000000417ACC8>>
input: "name"
('handler=', None)

发现convert_return_to_response的参数self.handler(request)的确为None
response = convert_return_to_response(self.handler(request), self.response_class)

根据print (handler)输出的ServiceServer.callback ，可以找到文件

C:\opt\ros\melodic\x64\lib\site-packages\rosserial_python
SerialClient.py

def callback(self, req):
        """ Forward request to serial device. """
        print("zzz20200215--handler--callback-----------------------------")
        data_buffer = StringIO.StringIO()
        req.serialize(data_buffer)
        print(self.id, data_buffer.getvalue())
        self.response = None
        print(self.parent.send)
        if self.parent.send(self.id, data_buffer.getvalue()) >= 0:
            print("send ok,wait....")
            while self.response is None:
                #print("isnone!!!!!!!!!!!!!")
                pass
            #print(self.response)
        return self.response

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

zzz 20200215
<class 'serialhwinfo.srv._hwinfo.hwinfoResponse'>
<bound method ServiceServer.callback of <rosserial_python.SerialClient.ServiceServer instance at 0x000000000474BD48>>
input: "name"
zzz20200215--handler--callback-----------------------------
(100, '\x04\x00\x00\x00name')
<bound method SerialClient.send of <rosserial_python.SerialClient.SerialClient object at 0x00000000038AA0F0>>
('handler=', None)

这个条件没有过呀

        if self.parent.send(self.id, data_buffer.getvalue()) >= 0:
            print("send ok,wait....")

parent.send 中parent是个什么鬼？原来是个SerialClient
<bound method SerialClient.send of <rosserial_python.SerialClient.SerialClient object at 0x00000000038AA0F0>>

接着找到

    def send(self, topic, msg):
        """
        Queues data to be written to the serial port.
        """
        print("===================")
        self.write_queue.put((topic, msg))

print大法有=====输出，证明是在这里了。

但是send并没有返回值？？？？也就是说返回None，这么神奇的事情？？？？看见_send有返回值，难道是版本升级导致？？？？好吧，
if self.parent.send(self.id, data_buffer.getvalue()) >= 0:这句情何以堪？没有返回做比较？

我：没有枪头你还捅？
ros：你怎么知道没有枪头就捅不死人？
反正我现在是想锤电脑了。。。

来吧，给你配个枪头先，加个返回值

def send(self, topic, msg):
"""
Queues data to be written to the serial port.
"""
print("===================")
self.write_queue.put((topic, msg))
return 1

输出

zzz 20200215
<class 'serialhwinfo.srv._hwinfo.hwinfoResponse'>
<bound method ServiceServer.callback of <rosserial_python.SerialClient.ServiceServer instance at 0x00000000041EBDC8>>
input: "name"
zzz20200215--handler--callback-----------------------------
(100, '\x04\x00\x00\x00name')
<bound method SerialClient.send of <rosserial_python.SerialClient.SerialClient object at 0x00000000033AA0F0>>
===================
send ok,wait....
('handler=', output: "SCREEN1602-PCF8574")
zzz20200215--handler--callback-----------------------------
(100, '\x04\x00\x00\x00name')
<bound method SerialClient.send of <rosserial_python.SerialClient.SerialClient object at 0x00000000033AA0F0>>
===================
send ok,wait....
('seq=', 10)
<cStringIO.StringO object at 0x0000000004216B20>

。。。
。。。

一切正常。
。。。
。。。

此时

内心波涛之涌转为止水之静。

顺便给官方提个建议

https://github.com/ros-drivers/rosserial/pulls

Update SerialClient.py #465