writing a service to publish only once in a latched topic
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()