Ask Your Question
0

How to subscribe two topics and publish a topic?

asked 2018-11-28 07:06:07 -0600

jql gravatar image

updated 2018-11-28 07:07:29 -0600

This is my publisher's code, the node is desired.

#!usr/bin/env python
import rospy
from std_msgs.msg import Float32

def desired():

pub = rospy.Publisher('desiredcounts', Float32, queue_size=10)
rospy.init_node('desired', anonymous=True)
    rate = rospy.Rate(0.1) # 10hz
    while not rospy.is_shutdown():
        desiredv = 6000  %rospy.get_time()
        rospy.loginfo(desiredv)
        pub.publish(desiredv)
        rate.sleep()

if __name__ == '__main__':
    try:
        desired()
    except rospy.ROSInterruptException:
pass

This is the subscriber's code that need to subscribe two topics and publish a topic base the receive messages.


#!usr/bin/env python

import rospy
from std_msgs.msg import Float32

def callback(data):
    rospy.loginfo('gotdesiredcounts %f', data.data)
    rospy.loginfo('currentencodercounts %f', data.data)

def gotdesired():
    rospy.init_node('gotdesired', anonymous=True)
    rospy.Subscriber('desiredcounts', Float32, callback)
    rospy.Subscriber('my_encodermotor', Float32, callback) 

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()
if __name__ == '__main__':
     gotdesired()    



def gottdesired():
    pub = rospy.Publisher('controlvalue', Float32, queue_size=10 

    rospy.init_node('gotdesired', anonymous=True)
    rate = rospy.Rate(0.1) # 10hz
    while not rospy.is_shutdown():
         kp=1
         kd=0.1
         dt=10     
         PDvalue = kp * ( desiredv - encoderv ) + kd *( desiredv - encoderv ) / dt
         rospy.loginfo(PDvalue)
         pub.publish(PDvalue)
         rate.sleep()      

if __name__ == '__main__':
    try:
        gottdesired()
    except rospy.ROSInterruptException:
        pass


When i run this two code, i can not receive the published message by node gotdeisred. Also error: global name 'desiredv' is not defined. Could you give me some advice or examples? Thanks very much.


Qiaoli Ji

edit retag flag offensive close merge delete

Comments

desiredv is referenced in your second node but never given a value = undefined. So that file doesn't run and your callback doesn't run. Maybe I am misunderstanding the question.

billy gravatar imagebilly ( 2018-11-28 14:45:01 -0600 )edit

Thanks for replying. I will try again.

jql gravatar imagejql ( 2018-11-29 08:19:53 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
4

answered 2018-11-28 15:52:22 -0600

RDaneelOlivaw gravatar image

Hi,

As far as I interpreted your question and for what I see, you wanted to create a control signal publisher based on desired value data and the encoder data.

I restructured your code and created a subscriber that published the control signal based on those tow data sources. Essentially the tricky part is seeing that we have to have two call backs for our data, one of them has inside the publisher of the control value.

I also created two dummy publisher to simulate the use of these topics and test the subscriber.

Here you have the files used, you basically have to launch the main.launch to make this work:

main.launch

<?xml version="1.0" encoding="UTF-8"?>
<launch> 

<node   name="publisher_desiredcounts"
        pkg="twosubs_onepub_pkg"
        type="publisher_desiredcounts.py"
        args=""
        output="screen"/>

<node   name="publisher_encoder"
        pkg="twosubs_onepub_pkg"
        type="publisher_encoder.py"
        args=""
        output="screen"/>

<node   name="subscriber"
        pkg="twosubs_onepub_pkg"
        type="subscriber.py"
        args=""
        output="screen"/>
</launch>

susbcriber.py

  #!/usr/bin/env python
  import rospy
  from std_msgs.msg import Float32
  from publisher_controlvalue import ControlValuePub

  class MySubscriber(object):

   def __init__(self):

    # TODO : do a wait topic        
    self.encoder = 0.0
    self.desiredcounts = 0.0


    rospy.Subscriber('desiredcounts', Float32, self.desiredcounts_callback)
    rospy.Subscriber('encoder', Float32, self.encoder_callback)
    self.contr_obj = ControlValuePub(kp=1, kd=0.1, dt=10)


def encoder_callback(self,msg):
    rospy.loginfo('got encoder %f', msg.data)
    self.encoder = msg.data

def desiredcounts_callback(self,msg):
    # This callback is the boss, this one dictates the publish rate
    rospy.loginfo('got desiredcounts %f', msg.data)
    self.desiredcounts = msg.data
    self.contr_obj.publish_controlvalue(desiredv=self.desiredcounts,
                                        encoderv=self.encoder)


def loop(self):
    rospy.logwarn("Starting Loop...")
    rospy.spin()

if __name__ == '__main__':
    rospy.init_node('subscriber_node', anonymous=True, log_level=rospy.WARN)
    my_subs = MySubscriber()
    my_subs.loop()

publisher_controlvalue.py

#!/usr/bin/env python
import rospy
from std_msgs.msg import Float32


class ControlValuePub(object):

    def __init__(self, kp=1, kd=0.1, dt=10):
        self._kp = kp
        self._kd = kd
        self._dt = dt
        self.pub = rospy.Publisher('controlvalue', Float32, queue_size=10) 

    def publish_controlvalue(self,desiredv, encoderv):

        PDvalue = self._kp * ( desiredv - encoderv ) + self._kd *( desiredv - encoderv ) / self._dt
        rospy.loginfo(PDvalue)
        self.pub.publish(PDvalue)
        rospy.logwarn("PUB Control Value="+str(PDvalue))

if __name__ == '__main__':
    rospy.init_node('controlvalue_node', anonymous=True, log_level=rospy.INFO)
    contr_obj = ControlValuePub()
    try:
        contr_obj.publish_controlvalue(desiredv=1.0, encoderv=2.0)
    except rospy.ROSInterruptException:
        pass

publisher_desiredcounts.py

#!/usr/bin/env python
import rospy
from std_msgs.msg import Float32
from math import sin

def desiredcounts():
    rospy.init_node('desiredcounts_node', anonymous=True, log_level=rospy.WARN)
    pub = rospy.Publisher('desiredcounts', Float32, queue_size=10)

    rate = rospy.Rate(10) # 10hz
    while not rospy.is_shutdown():
        desiredv = sin(rospy.get_time())
        rospy.loginfo(desiredv)
        pub.publish(desiredv)
        rate.sleep()

if __name__ == '__main__':
    try:
        desiredcounts()
    except rospy.ROSInterruptException:
        pass

publisher_encoder.py

#!/usr/bin/env python
import rospy
from std_msgs.msg import Float32
from math import cos

def encoder():
    rospy.init_node('encoder_node', anonymous=True, log_level=rospy.WARN)
    pub = rospy.Publisher('encoder', Float32, queue_size=10)

    rate = rospy.Rate(10) # 10hz
    while not rospy.is_shutdown():
        encoderv = cos(rospy.get_time())
        rospy.loginfo(encoderv ...
(more)
edit flag offensive delete link more

Comments

Thanks very much. I think I need to make some explanations. The message of topic desiredcounts is a constant such as 5000.0 type float32. We get the encoderv of topic encoder by the publisher running on matlab-simulink-ROS which was installed on other computer. So the encoderv is variable with time

jql gravatar imagejql ( 2018-11-29 08:10:33 -0600 )edit

I will subscribe the topic published from simulink-ROS. But the question is that how to calculate the PDvalue, especially the KD term. Kd*(error(t)-error(t-1)), error(t)=desiredv(t)-encoderv(t). Additionly, why you add log_level in rospy.init_node? Could you add explanations for codes? Thanks a lot

jql gravatar imagejql ( 2018-11-29 08:17:03 -0600 )edit

Your Answer

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

Add Answer

Question Tools

1 follower

Stats

Asked: 2018-11-28 07:06:07 -0600

Seen: 1,285 times

Last updated: Nov 28 '18