Merge pull request #770 from cehberlin/fix_define_ros_publisher_queue…

…_size

ROS publisher queue_size

src/morse/middleware/ros/abstract_ros.py

@@ -85,14 +85,21 @@ def initialize(self):
         # do not create a topic if no ros_class set (for TF publish only)
         if self.ros_class != Header:
             # Generate a publisher for the component
-            self.topic = rospy.Publisher(topic_name, self.ros_class)
+            self.topic = rospy.Publisher(topic_name, self.ros_class, queue_size=self.determine_queue_size())
         if self.default_frame_id is 'USE_TOPIC_NAME': # morse convention
             self.frame_id = self.kwargs.get('frame_id', self.topic_name)
         else: # default_frame_id was overloaded in subclass
             self.frame_id = self.kwargs.get('frame_id', self.default_frame_id)
         self.sequence = 0 # for ROS msg Header
         logger.info('ROS publisher initialized for %s'%self)
 
+    def determine_queue_size(self):
+        """
+        Determine a suitable queue_size for the ros publisher
+        :return: queue_size
+        """
+        return max(1, self.component_instance.frequency)
+
     def get_ros_header(self):
         header = Header()
         header.stamp = self.get_time()
@@ -119,7 +126,7 @@ class ROSPublisherTF(ROSPublisher):
     def initialize(self):
         ROSPublisher.initialize(self)
         if not ROSPublisherTF.topic_tf:
-            ROSPublisherTF.topic_tf = rospy.Publisher("/tf", tfMessage)
+            ROSPublisherTF.topic_tf = rospy.Publisher("/tf", tfMessage, queue_size=self.determine_queue_size())
 
     def finalize(self):
         ROSPublisher.finalize(self)

src/morse/middleware/ros/video_camera.py

@@ -21,7 +21,8 @@ def initialize(self):
         else:
             ROSPublisher.initialize(self)
         # Generate a publisher for the CameraInfo
-        self.topic_camera_info = rospy.Publisher(self.topic_name+'/camera_info', CameraInfo)
+        self.topic_camera_info = rospy.Publisher(self.topic_name+'/camera_info', CameraInfo,
+                                                 queue_size=self.determine_queue_size())
 
     def finalize(self):
         if self.pub_tf:

src/morse/middleware/ros_request_manager.py

@@ -28,7 +28,7 @@ def ros_timer(callable_obj, frequency):
             rospy.logdebug("Sleep interrupted")
 
 
-class RosAction:
+class RosAction(object):
     """ Implements a minimal action state machine.
 
     See http://www.ros.org/wiki/actionlib/DetailedDescription
@@ -57,17 +57,19 @@ def __init__(self, manager, component, action, rostype):
         self.result_type = types[1]
         self.feedback_type = types[2]
 
+        # read the frequency with which to publish status from the parameter server
+        # (taken from actionlib/action_server.py)
+        self.status_frequency = rospy.get_param(self.name + "/status_frequency", 5.0)
+        queue_size = max(1, self.status_frequency)
+
         # Create the 5 topics required by an action server
         self.goal_topic = rospy.Subscriber(self.name + "/goal", types[0], self.on_goal)
-        self.result_topic = rospy.Publisher(self.name + "/result", types[1])
-        self.feedback_topic = rospy.Publisher(self.name + "/feedback", types[2])
+        self.result_topic = rospy.Publisher(self.name + "/result", types[1], queue_size=queue_size)
+        self.feedback_topic = rospy.Publisher(self.name + "/feedback", types[2], queue_size=queue_size)
 
         self.cancel_topic = rospy.Subscriber(self.name + "/cancel", actionlib_msgs.msg.GoalID, self.on_cancel)
-        self.status_topic = rospy.Publisher(self.name + "/status", actionlib_msgs.msg.GoalStatusArray)
-
-        # read the frequency with which to publish status from the parameter server
-        # (taken from actionlib/action_server.py)
-        self.status_frequency = rospy.get_param(self.name + "/status_frequency", 5.0)
+        self.status_topic = rospy.Publisher(self.name + "/status", actionlib_msgs.msg.GoalStatusArray,
+                                            queue_size=queue_size)
 
         status_list_timeout = rospy.get_param(self.name + "/status_list_timeout", 5.0)
         self.status_list_timeout = rospy.Duration(status_list_timeout)
@@ -181,8 +183,8 @@ def publish_result(self, goal_id, result):
         with self.goal_lock:
             goal_status = self._pending_goals[goal_id]['status']
 
-        res = self.result_type(status = goal_status,
-                               result = result)
+        res = self.result_type(status=goal_status,
+                               result=result)
 
         self.result_topic.publish(res)