ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Python Not able to echo ROS2 topics published on thread

asked 2020-07-08 13:49:04 -0500

Malz gravatar image

updated 2020-07-08 21:07:55 -0500

I have a node with two topics i am publishing i created two threads and publishing them every 10 ms however when i list topics i could see the topics but when i subscribe or echo i don't get anything

import rclpy
from rclpy.node import Node
import threading
from std_msgs.msg import String
from sensor_msgs.msg import Image
from collections import deque
from rclpy.node import Node

rclpy.init()
g_node = rclpy.create_node('node_name') 
rgb_image_queue = deque()
depth_image_deque = deque()

g_queue_size = 1
topic_name = "/test_topic"
topic_name2 = "/test_topic"

def rgb_img_publisher_thread() -> None:
   publisher_ = g_node.create_publisher(String, topic_name, g_queue_size)
   i = 0
   while True:
       rclpy.spin_once(g_node) 
       # image_raw_ = depth_image_deque.pop()
       msg = String()
       msg.data = 'Test Img Publisher: %d' % i
       i += 1                                                       
       publisher_.publish(msg)  

def depth_img_publisher_thread() -> None:                                                         
   publisher_ = g_node.create_publisher(String, topic_name2, g_queue_size)
   i = 0
   while True:
       rclpy.spin_once(g_node)                                                                                    
       # image_raw_ = rgb_image_queue.pop()
       msg = String()
       msg.data = 'Test Img Publisher: %d' % i
       i += 1                                                       
       publisher_.publish(msg)                                                            


def main(args=None):

   while rclpy.ok(): 
       print("here ")
       rclpy.spin_once(g_node) 
       thread2 = threading.Thread(target=rgb_img_publisher_thread())
       thread1 = threading.Thread(target=depth_img_publisher_thread())

       thread2.start()
       thread1.start()
       thread2.join()
       thread1.join()


rclpy.shutdown()


if __name__ == '__main__':
    main()

The following code works but the threads are not concurrent. they are executing serially not concurrent.

def main1(args=None):
    i = 0
    while rclpy.ok():
        publisher1 = g_node.create_publisher(String, '/topic1', g_queue_size)
        publisher2 = g_node.create_publisher(String, '/topic2', g_queue_size)
        msg = String()
        i += 1
        msg.data = 'Test Img Publisher: %d' % i
        thread2 = threading.Thread(publisher2.publish(msg))
        thread1 = threading.Thread(publisher1.publish(msg))
        thread2.start()
        thread1.start()
        thread2.join()
        thread1.join()

if __name__ == '__main__':
    main1()
edit retag flag offensive close merge delete

Comments

1

Please post your complete code. Where do you call spin()?

Geoff gravatar image Geoff  ( 2020-07-08 18:53:06 -0500 )edit

@Geoff I posted the complete code. I was actually trying to call the publisher in a thread but on the same node.

enter code here

Malz gravatar image Malz  ( 2020-07-08 21:08:05 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2020-07-09 19:19:02 -0500

Geoff gravatar image

You are calling spin_once() in each thread. The deep implementation of that function includes locks to prevent concurrent access, so what is probably happening is that your two threads are deadlocking when they try to call spin_once() together.

To make your code work, you should call spin() in your main() after constructing and launching the threads.

To make your code better, I think you should stop using the threads unless you have a good reason to do so. A better approach in ROS 2 is to create two timers, one for each publisher, and then call spin() and let the executor handle scheduling the timers so that your publish() calls get made at the right times. Have a look at the Python sample, which uses a timer to do some work.

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2020-07-08 13:49:04 -0500

Seen: 905 times

Last updated: Jul 09 '20