ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

writing a service to publish only once in a latched topic

asked 2018-08-05 02:06:55 -0500

ShehabAldeen gravatar image

Hello,

I am writing a Service which should save a value from a topic to another latched topic so I can use that value later

I am using the following code and my topic seems to be not connected to the publisher since I am getting 0 connections always from the get_num_connections()

    #!/usr/bin/env python

import rospy
from std_msgs.msg import Float64
from std_srvs.srv import Empty, EmptyResponse


class save_hdg(object):

    def __init__(self):
        self.init_hdg()
        self.latch_publisher = rospy.Publisher('/autonomous/saved_heading',
                                               Float64,queue_size=10,latch=False)
        self.serv = rospy.Service('/autonomous/save_heading',Empty, self.publish_once_only)
        self.hdg_subscriber = rospy.Subscriber('/mavros/global_position/compass_hdg', Float64,
                                               self.subscriber_callback, queue_size=1)
        self.rate = rospy.Rate (10)
        rospy.loginfo("The save heading service is ready")
        self.connections = self.latch_publisher.get_num_connections()



    def init_hdg(self):
        self._hdg = None
        while self._hdg is None:
            try:
                self._hdg = rospy.wait_for_message("/mavros/global_position/compass_hdg",
                                                   Float64, timeout=1)
            except:
                rospy.loginfo("/compass_hdg topic is not ready yet, retrying")

        rospy.loginfo("/compass_hdg topic READY")

    def publish_once_only(self,req):
        rospy.loginfo("save heading service has been called")
        self.hdg = self.current_heading
        try:
            while not rospy.is_shutdown():
                rospy.loginfo("Connections: %d",self.connections)
                rospy.loginfo("Current heading is %f",self.hdg)
                if self.connections > 0 :
                    self.latch_publisher.publish(self.hdg)
                    rospy.loginfo("Published")
                    break
                self.rate.sleep()
        except rospy.ROSInterruptException:
            raise e

        return EmptyResponse()

    def subscriber_callback(self,msg):
        self.current_heading = msg.data

if __name__ == "__main__":
    rospy.init_node("save_heading_ServiceServer")
    save_heading_object = save_hdg()
    rospy.spin()
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2018-08-05 03:07:40 -0500

NEngelhard gravatar image

You always get self.connections == 0 because you don't update the value within your while loop. But this is not the main issue with your code.

How I understand you question is that you want to have a node that subscribes to a topic and stores the current heading if a service is triggered and afterwards provides this selected heading to all nodes that subscribe to a second topic. In this case, your code could look like this:

A latched publisher automatically sends its current message to any new subscriber.

class save_hdg(object):

    def __init__(self):
        self.latch_publisher = rospy.Publisher('/autonomous/saved_heading',
                                               Float64, queue_size=10, latch=True) # TURNING ON LATCHING

        self.serv = rospy.Service('/autonomous/save_heading', Empty, self.publish_once_only)

        self.hdg_subscriber = rospy.Subscriber('/mavros/global_position/compass_hdg', Float64,
                                               self.subscriber_callback, queue_size=1)

        # initialize all member variables in init
        self.heading_received = False
        self.current_heading = self.hdg = None
        rospy.loginfo("The save heading service is ready")

    def publish_once_only(self, req):
        rospy.loginfo("save heading service has been called")

        if self.heading_received:
            self.hdg = self.current_heading
            self.latch_publisher.publish(self.hdg)
        else:
            rospy.logwarn("Did not receive heading yet, can't republish")

        return EmptyResponse()

    def subscriber_callback(self, msg):
        self.heading_received = True
        self.current_heading = msg.data

if __name__ == "__main__":
    rospy.init_node("save_heading_ServiceServer")
    save_heading_object = save_hdg()
    rospy.spin()
edit flag offensive delete link more
0

answered 2018-08-05 03:46:35 -0500

ShehabAldeen gravatar image

Actually, I don't know maybe I am using the get_num_connections wrong but at the end, I have erased it and just making the function simple like that now it's working thank you, I appreciate your help

def publish_once_only(self,req):
    rospy.loginfo("save heading service has been called")
    self.hdg = self.current_heading
    try:
        self.latch_publisher.publish(self.hdg)
        rospy.loginfo("Published")
        self.rate.sleep()
    except rospy.ROSInterruptException:
        raise e

    return EmptyResponse()
edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2018-08-05 02:06:55 -0500

Seen: 939 times

Last updated: Aug 05 '18