Revision history [back]

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)
        pub.publish(encoderv)
        rate.sleep()

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

Hope it helped.

Here you have the git with all the code: GIT

Here you have the ROSJECT

And here a Video Explaining a bit more in detail everything: VIDEO