fishbot小车的odom话题消息因为qos不对而接收不了

[siviechen]

我想要自己写一个节点来对fishbot小车发布的odom数据进行evo文件排版的储存，但是终端提示qos策略不对，/odom话题是best_effort。以下是此种情况的代码和日志：

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy

class NodeToEvo(Node):
    def __init__(self,name):
        super().__init__(name)

        self.get_logger().info("transformation starts")
        self.subscribe = self.create_subscription(Odometry,"odom",self.callback,10)
        self.path = "/home/cxw"
        self.file = self.path + "/ground_truth_odom" + '.txt'
        self.writter = open(self.file,'w')


    def callback(self,msg):
        position = msg.pose.pose.position
        orientation = msg.pose.pose.orientation
        self.writter.write(position.x+'\t'+position.y+'\t'+position.z+'\t'+orientation.x+'\t'+orientation.y+'\t'+orientation.z+'\t'+orientation.w+'\n')


def main(args=None):
    rclpy.init(args=args)
    node = NodeToEvo("ToEvo")
    rclpy.spin(node)
    node.destroy_node() 
    rclpy.shutdown()

然后启动microros接收小车的数据

sudo docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged --net=host microros/micro-ros-agent:$ROS_DISTRO udp4 --port 8888 -v6

编译和source完后，ros2 run前面的py程序，日志报错如下：

cxw@pc:~/dev_ws$ ros2 run learning_topic toEvo 
[INFO] [1691715773.163893894] [ToEvo]: transformation starts
[WARN] [1691715774.291494228] [ToEvo]: New publisher discovered on topic 'odom', offering incompatible QoS. No messages will be received from it. Last incompatible policy: RELIABILITY

然后查看小车odom的info：

cxw@pc:~/dev_ws$ ros2 topic info --verbose /odom 
Type: nav_msgs/msg/Odometry

Publisher count: 1

Node name: fishbot_motion_control
Node namespace: /
Topic type: nav_msgs/msg/Odometry
Endpoint type: PUBLISHER
GID: 01.0f.bc.e6.2d.00.ca.28.01.00.00.00.00.00.01.03.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: BEST_EFFORT
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Subscription count: 0

然后我去网上查，查到了一些在程序里修改qos策略的贴子，但是还是没有跑起来。修改后的代码:

import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy

class NodeToEvo(Node):
    def __init__(self,name):
        super().__init__(name)

        qos_profile = QoSProfile(
			reliability = QoSReliabilityPolicy.BEST_EFFORT,
			history = QoSHistoryPolicy.UNKNOWN,
            depth = 10
		)

        self.get_logger().info("transformation starts")
        self.subscribe = self.create_subscription(Odometry,"odom",self.callback,qos_profile)
        self.path = "/home/cxw"
        self.file = self.path + "/ground_truth_odom" + '.txt'
        self.writter = open(self.file,'w')


    def callback(self,msg):
        position = msg.pose.pose.position
        orientation = msg.pose.pose.orientation
        self.writter.write(position.x+'\t'+position.y+'\t'+position.z+'\t'+orientation.x+'\t'+orientation.y+'\t'+orientation.z+'\t'+orientation.w+'\n')


def main(args=None):
    rclpy.init(args=args)
    node = NodeToEvo("ToEvo")
    rclpy.spin(node)
    node.destroy_node() 
    rclpy.shutdown()

编译后运行的终端log：

cxw@pc:~/dev_ws$ ros2 run learning_topic toEvo 
[INFO] [1691716079.532903710] [ToEvo]: transformation starts

>>> [rcutils|error_handling.c:108] rcutils_set_error_state()
This error state is being overwritten:

  'Unknown QoS history policy, at ./src/qos.cpp:61'

with this new error message:

  'create_publisher() failed setting topic QoS, at ./src/subscription.cpp:221'

rcutils_reset_error() should be called after error handling to avoid this.
<<<

>>> [rcutils|error_handling.c:108] rcutils_set_error_state()
This error state is being overwritten:

  'create_publisher() failed setting topic QoS, at ./src/subscription.cpp:221, at ./src/rcl/subscription.c:108'

with this new error message:

  'invalid allocator, at ./src/rcl/subscription.c:218'

rcutils_reset_error() should be called after error handling to avoid this.
<<<
invalid allocator, at ./src/rcl/subscription.c:218
Traceback (most recent call last):
  File "/home/cxw/dev_ws/install/learning_topic/lib/learning_topic/toEvo", line 33, in <module>
    sys.exit(load_entry_point('learning-topic==0.0.0', 'console_scripts', 'toEvo')())
  File "/home/cxw/dev_ws/install/learning_topic/lib/python3.10/site-packages/learning_topic/toEvo.py", line 34, in main
    node = NodeToEvo("ToEvo")
  File "/home/cxw/dev_ws/install/learning_topic/lib/python3.10/site-packages/learning_topic/toEvo.py", line 20, in __init__
    self.subscribe = self.create_subscription(Odometry,"odom",self.callback,qos_profile)
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/node.py", line 1370, in create_subscription
    subscription_object = _rclpy.Subscription(
rclpy._rclpy_pybind11.RCLError: Failed to create subscription: invalid allocator, at ./src/rcl/subscription.c:218
[ros2run]: Process exited with failure 1

这下我就找不到解答这个的贴子了，求大佬指教

[小鱼]

@siviechen 这样写

from nav_msgs.msg import  Odometry
self.subscription = self.create_subscription(
            Odometry,
            '/odom',
            self.listener_callback,
            rclpy.qos.qos_profile_sensor_data)

[siviechen]

@小鱼 成功了，感谢小鱼

[siviechen]

@小鱼 这个语句是指自动去匹配发布者的qos吗？

[小鱼]

@siviechen 不是，传感器类型是默认的best_effort