Ask Your Question
0

tf broadcaster does not work in another process

asked 2017-02-03 05:23:34 -0500

psi gravatar image

updated 2017-02-03 05:47:49 -0500

I want to communicate with the main process running STM and odometry publisher (another process) using Python multiprocessing module. I have got two queues input_command_queue and lz_queue. The problem is that the tf broadcaster does not transform between 'base_link' and 'odom' frames as written below. I've found out it using 'rosrun tf view_frames' command. It will transform only if it is a separate node. But I don't want make it as a separate node, because then I have to do listening and publishing between these two nodes, haven't I? How to make tf_transform be working?

main.py

#!/usr/bin/env python
import rospy
from driver import Driver
from multiprocessing import Process, Queue
import odometry

rospy.init_node('stm_node')

input_command_queue = Queue()
lz_queue = Queue()
fsm_queue = Queue()

odom = Process(target=odometry.odom_publisher, args=(input_command_queue, lz_queue))
odom.start()

d = Driver(input_command_queue, fsm_queue, lz_queue)
d.start()
d.join()
odom.join()

odometry.py

import tf
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3
import multiprocessing
import rospy

def odom_publisher(input_command_queue, lz_queue):

    #rospy.init_node('odometry_publisher')

    odom_pub = rospy.Publisher("odom", Odometry, queue_size=50)
    odom_broadcaster = tf.TransformBroadcaster()
    r = rospy.Rate(1.0)

    while not rospy.is_shutdown():
        current_time = rospy.Time.now()
        command = {'source': 'localization', 'cmd': 'getCurentCoordinates', 'params': ''}
        input_command_queue.put(command)
        x, y, th = lz_queue.get()['data'] # todo
        print x, y, th
        # since all odometry is 6DOF we'll need a quaternion created from yaw
        odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)
        # first, we'll publish the transform over tf
        odom_broadcaster.sendTransform((x, y, 0.), odom_quat, current_time, "base_link","odom")
        # next, we'll publish the odometry message over ROS
        odom = Odometry()
        odom.header.stamp = current_time
        odom.header.frame_id = "odom"
        # set the position
        odom.pose.pose = Pose(Point(x, y, 0.), Quaternion(*odom_quat))
        odom.child_frame_id = "base_link"
        # set the velocity
        #odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth))
        # publish the message
        odom_pub.publish(odom)

        r.sleep()
edit retag flag offensive close merge delete

Comments

I don't completely understand what you're trying to do, but IIRC you must have a rospy.init_node(..) somewhere before using any rospy functionality. I don't see it in the code you show.

gvdhoorn gravatar image gvdhoorn  ( 2017-02-03 05:39:30 -0500 )edit

yes, I've forgot to include it into the main.py. Now it is updated.

psi gravatar image psi  ( 2017-02-03 05:48:46 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2017-02-04 01:19:34 -0500

psi gravatar image

It seems to be solved. It was obvious to transport broadcasting code to stm_node.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2017-02-03 05:22:07 -0500

Seen: 226 times

Last updated: Feb 04 '17