[ROS2 foxy] QoS durability transient_local rclpy to echo /robot_description
I woud like to subscribe to the robot_description coming from the robot state publisher and then from that derive the tf_static since it is supposed to be a urdf in String form. I am sure that the robot_description topic is published with the QoS durability option set to transient_local.
Robot state publisher
The robot state publisher works ok since I can see the output from it and it runs ok. I can also echo /tf_static.
[robot_state_publisher.EXE-1] Parsing robot urdf xml string.
[robot_state_publisher.EXE-1] Link LDS-01 had 6 children
[robot_state_publisher.EXE-1] Link solid(1)_707 had 0 children
[robot_state_publisher.EXE-1] Link screw0_746 had 0 children
[robot_state_publisher.EXE-1] Link screw1_793 had 0 children
[robot_state_publisher.EXE-1] Link screw2_840 had 0 children
[robot_state_publisher.EXE-1] Link screw3_887 had 0 children
[robot_state_publisher.EXE-1] Link solid_722 had 0 children
[robot_state_publisher.EXE-1] Link imu_link had 0 children
[robot_state_publisher.EXE-1] Link left wheel had 0 children
[robot_state_publisher.EXE-1] Link right wheel had 0 children
[robot_state_publisher.EXE-1] [INFO] [1612966920.980180100] [robot_state_publisher]: got segment LDS-01
[robot_state_publisher.EXE-1] [INFO] [1612966920.980240900] [robot_state_publisher]: got segment base_link
[robot_state_publisher.EXE-1] [INFO] [1612966920.980262100] [robot_state_publisher]: got segment imu_link
[robot_state_publisher.EXE-1] [INFO] [1612966920.980273600] [robot_state_publisher]: got segment left wheel
[robot_state_publisher.EXE-1] [INFO] [1612966920.980284000] [robot_state_publisher]: got segment right wheel
[robot_state_publisher.EXE-1] [INFO] [1612966920.980295000] [robot_state_publisher]: got segment screw0_746
[robot_state_publisher.EXE-1] [INFO] [1612966920.980304500] [robot_state_publisher]: got segment screw1_793
[robot_state_publisher.EXE-1] [INFO] [1612966920.980314300] [robot_state_publisher]: got segment screw2_840
[robot_state_publisher.EXE-1] [INFO] [1612966920.980323900] [robot_state_publisher]: got segment screw3_887
[robot_state_publisher.EXE-1] [INFO] [1612966920.980330500] [robot_state_publisher]: got segment solid(1)_707
[robot_state_publisher.EXE-1] [INFO] [1612966920.980340800] [robot_state_publisher]: got segment solid_722
Output of ros2 topic info /robot_description --verbose
:
Publisher count: 1
Node name: robot_state_publisher
Node namespace: /
Topic type: std_msgs/msg/String
Endpoint type: PUBLISHER
GID: 01.0f.af.50.f4.2e.00.00.01.00.00.00.00.00.15.03.00.00.00.00.00.00.00.00
QoS profile:
Reliability: RMW_QOS_POLICY_RELIABILITY_RELIABLE
Durability: RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
Lifespan: 2147483651294967295 nanoseconds
Deadline: 2147483651294967295 nanoseconds
Liveliness: RMW_QOS_POLICY_LIVELINESS_AUTOMATIC
Liveliness lease duration: 2147483651294967295 nanoseconds
Subscription count: 0
Echoing attempt
ros2 topic echo --qos-durability=transient_local /robot_description std_msgs/msg/String
I have tried also:
ros2 topic echo --qos-durability=transient_local /robot_description
But none of these worked
Subscription attempt
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy
class WebotsStatusPublisher(Node):
def __init__(self):
super().__init__('docker_bridge_node')
self.latching_qos=QoSProfile(depth=1,reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE,durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL)
self.robot_desc_subscription = self.create_subscription(String,'robot_description',self.robot_desc_listener_callback, self.latching_qos)
self.robot_desc_publisher = self.create_publisher(String,'robot_description_bridge', self.latching_qos)
self.robot_desc_subscription
def robot_desc_listener_callback(self, msg):
print("I got the robot description")
self.robot_desc_publisher.publish(msg)
def main(args=None):
rclpy.init(args=args)
webots_status = WebotsStatusPublisher()
rclpy.spin(webots_status)
webots_status.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
And it is still not possible to subscribe to it. I have checked the documentation both for the robot state publisher (https://index.ros.org/r/robot_state_p...) and for the quality of service profile (http://docs.ros2.org/latest/api/rclpy...) and it seems to me that I am ...