arduino設備跑 ros service server 的波折記。
引——
參考前文https://blog.csdn.net/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
記——
外面陽光明媚，春光大好，只有疫情讓人憋屈，但對於猿們來說，程序搗鼓通了，隔着窗也能體會到春風拂面的那種感覺！
此文旨在遇坑做個記號！有用給個贊！謝謝！