Move topic advertising to initialization phase

Author: gonzalocasas

src/compas_fab/backends/ros/client.py

@@ -81,12 +81,19 @@ def __init__(self, host='localhost', port=9090, is_secure=False, planner_backend
         planner_backend_type = PLANNER_BACKENDS[planner_backend]
         self.__class__ = type('RosClient_' + planner_backend_type.__name__, (planner_backend_type, RosClient), {})
 
+
     def __enter__(self):
         self.run()
         self.connect()
+
+        # Planners usually need to initialize/advertise topics and/or services
+        self.init_planner()
+
         return self
 
     def __exit__(self, *args):
+        self.dispose_planner()
+
         self.close()
 
     def inverse_kinematics(self, frame, base_link, group,

src/compas_fab/backends/ros/planner_backend_moveit.py

@@ -92,6 +92,20 @@ class MoveItPlanner(PlannerBackend):
                                             GetPlanningSceneRequest,
                                             GetPlanningSceneResponse)
 
+    def init_planner(self):
+        self.collision_object_topic = Topic(self, '/collision_object',
+                                            'moveit_msgs/CollisionObject', queue_size=None)
+        self.collision_object_topic.advertise()
+
+        self.attached_collision_object_topic = Topic(
+            self, '/attached_collision_object',
+            'moveit_msgs/AttachedCollisionObject', queue_size=None)
+        self.attached_collision_object_topic.advertise()
+
+    def dispose_planner(self):
+        self.collision_object_topic.unadvertise()
+        self.attached_collision_object_topic.unadvertise()
+
     # ==========================================================================
     # planning services
     # ==========================================================================
@@ -286,8 +300,7 @@ def append_collision_mesh(self, collision_mesh):
 
     def _collision_object(self, collision_object, operation=CollisionObject.ADD):
         collision_object.operation = operation
-        topic = Topic(self, '/collision_object', 'moveit_msgs/CollisionObject')
-        topic.publish(collision_object.msg)
+        self.collision_object_topic.publish(collision_object.msg)
 
     def add_attached_collision_mesh(self, attached_collision_mesh):
         """Add a collision mesh attached to the robot."""
@@ -303,6 +316,4 @@ def remove_attached_collision_mesh(self, id):
 
     def _attached_collision_object(self, attached_collision_object, operation=CollisionObject.ADD):
         attached_collision_object.object.operation = operation
-        topic = Topic(self, '/attached_collision_object',
-                      'moveit_msgs/AttachedCollisionObject')
-        topic.publish(attached_collision_object.msg)
+        self.attached_collision_object_topic.publish(attached_collision_object.msg)