Create a generic node in python
Hi all, I am trying to post this question once again. I am trying to create a ros node with multiple publishers, subscribers, services and action servers. To create this, I create a class and define functions for all publishers(using while loop), subscribers, services and actions. But when I call these functions using the object of the class in the main block, only the first called publisher runs(in the while loop), blocking all the other method calls.
The code is as below:
#!/usr/bin/env python
import roslib
import rospy
import sys
from std_msgs.msg import String
import actionlib
from smach_ros import ActionServerWrapper
from SKA.srv import *
from SKA.msg import *
from threading import Thread
from actionlib_msgs.msg import *
class CN_Dish():
def __init__(self):
rospy.init_node('cn_dish')
init_message = rospy.get_param('~message', 'cn_dish node started')
rate = float(rospy.get_param('~rate', '1.0'))
rospy.loginfo('rate = %d', rate)
#(Continuous Publisher)
def publisher_For_HealthMonitoring(self,event):
pub = rospy.Publisher('HealthMonitoring', HealthMonitoring_Message,queue_size=10)
rate = rospy.Rate(10) # 10hz
msg = HealthMonitoring_Message()
while not rospy.is_shutdown():
rospy.loginfo(msg)
pub.publish(msg)
#(Continuous Publisher)
def publisher_For_powerLevel(self,event):
pub = rospy.Publisher('powerLevel', PowerLevel_Message,queue_size=10)
rate = rospy.Rate(10) # 10hz
msg = PowerLevel_Message()
while not rospy.is_shutdown():
pub.publish(msg)
#(Single time publisher)
def publisher_For_ThresholdCrossed(self):
pub = rospy.Publisher('ThresholdCrossed', ThresholdCrossed_Message)
rate = rospy.Rate(10) # 10hz
msg = ThresholdCrossed_Message()
if not rospy.is_shutdown():
rospy.loginfo(msg)
pub.publish(msg)
rospy.spin()
#(Single time publisher)
def publisher_For_Pointing(self):
pub = rospy.Publisher('Pointing', Pointing_Message)
rate = rospy.Rate(10) # 10hz
msg = Pointing_Message()
if not rospy.is_shutdown():
rospy.loginfo(msg)
pub.publish(msg)
rospy.spin()
rate.sleep()
#(Subscriber for some external event)
def subscriber_for_event_ObserveStarted(self):
topic = rospy.get_param('~topic', 'ObserveStarted')
print "Subscription Started"
rospy.Subscriber(topic, ObserveStarted_Message ,self.callback_for_event_ObserveStarted)
# Create a callback function for the subscriber.
def callback_for_event_ObserveStarted(self,data):
# Simply print out values in our custom message.
rospy.loginfo(rospy.get_name() + " Event %s", data)
#Goal Action Server
def action_server_for_Point(self):
self._as = actionlib.SimpleActionServer('Point', Point_ActionAction,
execute_cb=self.handle_Point_action, auto_start = False)
self._as.start()
def handle_Point_action(self, goal):
r = rospy.Rate(1)
success = True
a = 0
while success:
print "Hello"
a = a+1
if(a>10):
success = False
r.sleep()
rospy.loginfo('Executing Goal ')
result = Point_ActionResult()
if True:
result.status = a
#Service
def handle_Stop_service(self,req):
print "Request Received %d"%(req.devNo)
return (0)
def service_server_for_Stop(self):
s = rospy.Service('Stop', Stop_Service, self.handle_Stop_service)
print "Ready Processing for Service."
rospy.spin()
if __name__ == '__main__':
# Initialize the node and name it.
# Go to class functions that do all the heavy lifting. Do error checking.
try:
ne = CN_Dish()
ne.(call all the function defined in the class to start publishing, subription, services and action server)
print "Done"
except rospy.ROSInterruptException: pass
Any hint or help would be a great help !!
That's no pseudo code. You have removed everything that could give a hint an what you do wrong...
Hi, Thanks for viewing the post. If I could explain my question again, I wish to create a ROS node which contains multiple publishers and subscribers and also multiple Service Servers and Action Servers. As for the code, I am very sorry that I can't post it in any more details
I don't think anyone will be able to help without seeing an example of the problem you're having.
Hi attached the code block, please see if you could suggest something which I am missing :)