Multiple subscribers and single publisher in one python script?

asked 2016-04-18 21:08:17 -0500

tercelkisor
13 ●3 ●4 ●7

updated 2016-05-11 16:17:26 -0500

I'm having issues with getting this to work. What I need to do is pull magnetometer data from the imu topic, pull gps data from gps topic, do calculations, and publish to cmd_vel topic. It seems like it would be simple, but I can't seem to get it to work. I can get both subscribers and the publisher to work simoultenously, but I cannot get the data to play together, and I'm pretty sure i'm getting stuck in a callback function.

My code is currently printing my bearing and heading, but it isn't moving, and I'm not sure what else I can try.

My most recent code:

    #!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Vector3Stamped
from geometry_msgs.msg import Vector3

from math import radians
from sensor_msgs.msg import NavSatFix
import time
import numpy

global lat1
global long1


def call_head(um6):

    global heading
    global bearing 

    r = rospy.Rate(5);
    # rospy.loginfo("Bearing: %s", um6.vector)          #Print mag reading for bearing
    z = um6.vector.z
    y = um6.vector.y

    angles = Vector3()

    heading = numpy.rad2deg(numpy.arctan2(z, y)) + 90

    angles.x = heading
    angles.z = bearing

    call_bear_pub = rospy.Publisher("navStuff", Vector3, queue_size=10)


    call_bear_pub.publish(angles)


    r.sleep()
    return "done heading"


def call_nav(navsat):

    global heading
    global bearing 

    global lat1
    global long1


    r = rospy.Rate(5);
    # rospy.loginfo("Latitude: %s", navsat.latitude)        #Print GPS co-or to terminal
    # rospy.loginfo("Longitude: %s", navsat.longitude)

    angles = Vector3()

    lat1 = navsat.latitude
    long1 = navsat.longitude

    ###########################################################
    lat2 = 30.210406
    #                                   Destination
    long2 = -92.022914
    ############################################################

    dLon = long2 - long1
    y = numpy.sin(dLon) * numpy.cos(lat2)
    x = numpy.cos(lat1)*numpy.sin(lat2) - numpy.sin(lat1)*numpy.cos(lat2)*numpy.cos(dLon)

    bearing = (numpy.rad2deg(numpy.arctan2(y, x)) + 360) % 360

    angles.z = bearing

    call_nav_pub = rospy.Publisher("navStuff", Vector3, queue_size=10)

    # if (heading != 0):
    #   call_nav_pub.publish(angles)
    # else:
    #   pass



    r.sleep()
    return "done bearing"

def call_move(data):


    r = rospy.Rate(5);

    pub = rospy.Publisher('/husky_velocity_controller/cmd_vel', Twist, queue_size=10)


    move_cmd = Twist()
    turn_cmd = Twist()

    ###########################################################
    lat2 = 30.210406
    #                                   Destination
    long2 = -92.022914
    ############################################################



    move_cmd.linear.x = 2
    turn_cmd.angular.z = radians(45)


    heading = data.z
    bearing = data.x



    turn_angle = heading - bearing

    rospy.loginfo("bearing: %s", bearing)
    rospy.loginfo("heading: %s", heading)


    if (turn_angle > 180):
        turn_angle -= 360
    elif (turn_angle < -180):
        turn_angle += 360
    else:
        turn_angle = turn_angle

    if (abs(lat1-lat2)<.0005 and abs(long1-long2)<.0005):
        pub.publish(Twist())
        r.sleep()
    else:
        if (abs(turn_angle) < 8):
            pub.publish(move_cmd)
            rospy.spin()
        else:
            pub.publish(turn_cmd)
            r.sleep()
    return "done move"      



def run():
    rospy.init_node('navigate_that_husky')
    rospy.Subscriber("/imu_um6/mag", Vector3Stamped, call_head)
    rospy.Subscriber("/gps/fix", NavSatFix, call_nav)
    rospy.Subscriber("navStuff", Vector3, call_move)

    rospy.spin()


if __name__ == '__main__':
    run()

That's actually what I tried first, bit I couldn't get it to work. I switched to this method because of this

I'm sure I just don't understand how to use global variables enough. I'll post a simple version my attempt at using global variables.

#!/usr/bin/env python

import rospy
from ...

(more)

edit retag flag offensive close merge delete

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2016-04-20 09:51:28 -0500

Sood
20 ●2 ●3 ●4

As suggested why don't you use a class and some flags


#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Vector3Stamped
from geometry_msgs.msg import Vector3

from math import radians
from sensor_msgs.msg import NavSatFix
import time
import numpy

global lat1
global long1

Class Foo: 

def call_head(um6):
    z = um6.vector.z
    y = um6.vector.y

    self.angles = Vector3()

    self.heading = numpy.rad2deg(numpy.arctan2(z, y)) + 90

    self.angles.x = self.heading
    self.angles.z = self.bearing

    self.send_bear_ = True
    return "done heading"


def call_nav(navsat):


    self.angles = Vector3()

    self.lat1 = navsat.latitude
    self.long1 = navsat.longitude

    dLon = self.long2 - self.long1
    y = numpy.sin(dLon) * numpy.cos(self.lat2)
    x = numpy.cos(lat1)*numpy.sin(self.lat2) - numpy.sin(self.lat1)*numpy.cos(self.lat2)*numpy.cos(dLon)

    self.bearing = (numpy.rad2deg(numpy.arctan2(y, x)) + 360) % 360

    self.angles.z = self.bearing

    self.send_nav_ = True

    return "done bearing"

def call_move(data):

    move_cmd = Twist()
    turn_cmd = Twist()



    move_cmd.linear.x = 2
    turn_cmd.angular.z = radians(45)

    self.heading = data.z
    self.bearing = data.x


    turn_angle = self.heading - self.bearing

    rospy.loginfo("bearing: %s", self.bearing)
    rospy.loginfo("heading: %s", self.heading)


    if (turn_angle > 180):
        turn_angle -= 360
    elif (turn_angle < -180):
        turn_angle += 360
    else:
        turn_angle = turn_angle

    if (abs(self.lat1-self.lat2)<.0005 and abs(self.long1-self.long2)<.0005):
        self.pub_msg = Twist()
    else:
        if (abs(turn_angle) < 8):
            self.pub_msg = move_cmd
        else:
            self.pub_msg = turn_cmd
    self.send_msg_ = True
    return "done move"      



def __init__(self):

    self.pub_msg = Twist()
    self.heading = #someinit val
    self.bearing = #someinit val
    self.send_msg_ = False
    self.send_bear_ = False
    self.send_ = False
    self.angles = Vector3()
    self.lat1 = #someinit val
    self.lat2 = #someinit val
    ###########################################################
    self.lat2 = 30.210406
    #                                   Destination
    self.long2 = -92.022914
    ############################################################

    rospy.Subscriber("/imu_um6/mag", Vector3Stamped, self.call_head)
    rospy.Subscriber("/gps/fix", NavSatFix, self.call_nav)
    rospy.Subscriber("navStuff", Vector3, self.call_move)
    pub = rospy.Publisher('/husky_velocity_controller/cmd_vel', Twist, queue_size=10)
    call_bear_pub = rospy.Publisher("navStuff", Vector3, queue_size=10)
    call_nav_pub = rospy.Publisher("navStuff", Vector3, queue_size=10)
    while not rospy.is_shutdown():
        if self.send_msg_:
            pub.publish(self.pub_msg)
            self.send_msg_ = False
        if self.send_bear_:
            call_bear_pub.publish(self.angles)
            self.send_bear_ = False
        if self.send_nav_:
            call_nav_pub.publish(self.angles)
            self.send_nav_ = False
 if __name__ == '__main__':
    rospy.init_node('navigate_that_husky')
    try:
        foo = Foo()
    except rospy.ROSInterruptException:  pass

Basically once you are receive the message the flag gets set and you can send it on the publisher. If you want them all together simply make sure that all must be true at the same time.

This should take care of timing issues. It did for me.

Sorry for the poor formatting I'm in a hurry.

edit flag offensive delete link

Comments

+1 for using a class. You could even get rid of the ugly global variables lat1 and long1. Another improvement in my eyes would be to remove the complete navStuff topic. Just call call_move() directly.

BennyRe ( 2016-04-21 01:54:58 -0500 )edit

I think I've decided to go this route. I've never used classes before, so I'll do a little reading on that first. Thanks.

tercelkisor ( 2016-04-24 14:48:43 -0500 )edit

Great! Did I answer your question successfully? Does it work now?

Sood ( 2016-05-22 13:54:26 -0500 )edit

add a comment

answered 2016-04-19 06:13:42 -0500

BennyRe
2949 ●16 ●30 ●54

updated 2016-04-20 01:56:45 -0500

Oh dear this code is a mess.

Have you read and UNDERSTOOD all tutorials under http://wiki.ros.org/ROS/Tutorials?

There are a few things wrong here.

You don't need all these rospy.Rate.
Don't create the publishers in your callbacks. Define them global and just publish in your callbacks.
Do other nodes need the data you publish on the navStuff topic? If not remove this whole topic from you program. There are much cheaper and easier ways to handle data inside your program e.g. store it in a global or member variable.

If I understand you right you need data from both /imu_um6/mag and /gps/fix. As soon as you got both you need to process it and publish to /husky_velocity_controller/cmd_vel. What you have to do is, store the data from these two topics in a global variable. In your run method you wait until you got your data and then process it and publish it.

Update

That already looks way better. I would recommend you to read some good Python tutorials. The learning curve of ROS itself is quite steep. But learning ROS and Python together can lead to heavy headache.

global lat 
global lon 

lat = navsat.latitude
lon = navsat.longitude

This is correct. You store the latitute and longitude in the global variables.

return lon
return lat

This is useless. 1. Callbacks don't process return values. 2. The second return is never reached since the function ends with the first return. (The correct way to do such stuff in Python would be return (lon, lat))

rospy.spin()

This call is blocking forever, so printer() is never invoked. Remove this rospy.spin()

Your printer()method could look like

def printer():
    while not (lat != 0 and lon != 0):
        rospy.sleep(.01)
    rospy.loginfo("Latitude: %s", lat)
    rospy.loginfo("Longitude: %s", lon)

edit flag offensive delete link

Comments

I've edited my question.

tercelkisor ( 2016-04-19 09:53:30 -0500 )edit

Note: I'd be surprised if lat or lon ever really will be == 0, seeing as they are floats.

gvdhoorn ( 2016-04-20 02:25:45 -0500 )edit

Yes I also thought about this. In this while I check if they got a value from the callback. At the time of declaration lat = 0 lon = 0 they are integers. As soon as the callback stores values in them they become floats and the while ends.

BennyRe ( 2016-04-20 02:50:05 -0500 )edit

So I removed the rospy.spin(), and the script just closes itself after printing "lat = 0 long = 0" I've read that the spin function is required to keep the script from "jumping out or something.

tercelkisor ( 2016-04-20 13:12:27 -0500 )edit

Did you insert the while loop?

BennyRe ( 2016-04-21 01:49:51 -0500 )edit

Yes, I tried the while loop. It sort of works, but it won't stop the script when I crtl+c.

tercelkisor ( 2016-04-24 14:47:49 -0500 )edit

while not (lat != 0 and lon != 0) and not rospy.is_shutdown():

should do the trick

BennyRe ( 2016-04-25 02:52:27 -0500 )edit

add a comment

Multiple subscribers and single publisher in one python script?

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Multiple subscribers and single publisher in one python script? edit

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Multiple subscribers and single publisher in one python script?