原创:rospy一个节点中实现订阅话题并发布消息到一个新话题
最近在写研究生论文,基于Auoware框架和ros平台的传感器融合算法实现。遇到以下问题,在一个节点中订阅一个话题,回调函数中处理之后将结果发布到另外一个话题上。笔者借鉴了 ros wiki上一个c++版本的实现思路,写了一个相对应的python版本。
这里感谢大神所提供的思路,原帖:https://answers.ros.org/question/59725/publishing-to-a-topic-via-subscriber-callback-function/。
下面是python版本的实现:
class SubscribeAndPublish:
def __init__(self):
self.__pub_ = rospy.Publisher('topic to be published',
Pub msg type, queue_size)
self.__sub_ = rospy.Subscriber('topic to be subscribed',
Sub msg type, self.callback)
def callback(self,data):
assert isinstance(data, Sub msg Type)
# To check if get correct msg
obj_toPub = Pub msg type()
# To create an instance of msg type to be published.
'''
这里加入对data数据的处理代码,data就是从订阅话题中获得的数据,
处理后存储到obj_toPub实例中,需要注意的是变量类型要对应,
否则没办法通过编译。
'''
self.__pub_.publish(obj_toPub)
def main():
rospy.init_node('node name', anonymous=True)
SAPObject = SubscribeAndPublish()
rospy.spin()
if __name__ == "__main__":
main()
经过目前的实践,证明可用,以及实现的功能有传感器的时空对齐。后面进行目标融合时应该也会用到,因为要订阅多个节点,最终只有一个输出。
第一次写文档,排版格式可能不够美观,大家谅解,代码中有不完美的地方请大家不吝赐教啦。。。