tf broadcaster does not work in another process
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()
I don't completely understand what you're trying to do, but IIRC you must have a
rospy.init_node(..)
somewhere before using anyrospy
functionality. I don't see it in the code you show.yes, I've forgot to include it into the main.py. Now it is updated.