Mir-bridge-ready-service with standard-srv-library (#12)

* Added mir_ready service as a workaround for lifecycle nodes (pythoN)

Author: niemsoen

mir_driver/mir_driver/mir_bridge.py

@@ -16,6 +16,7 @@
 from nav_msgs.msg import Odometry
 from sensor_msgs.msg import LaserScan
 from tf2_msgs.msg import TFMessage
+from std_srvs.srv import Trigger
 
 tf_prefix = ''
 
@@ -364,6 +365,10 @@ def callback(self, msg):
 class MiR100BridgeNode(Node):
     def __init__(self):
         super().__init__('mir_bridge')
+        
+        self.mir_bridge_ready = False #state
+        self.srv_mir_ready = self.create_service(Trigger, 'mir_bridge_ready', self.mir_bridge_ready_poll_callback)
+        
         try:
             hostname = self.declare_parameter('hostname', '192.168.12.20').value
         except KeyError:
@@ -409,6 +414,9 @@ def __init__(self):
             if ('/' + sub_topic.topic) not in subscribed_topics:
                 self.get_logger().warn(
                     "Topic '%s' is not yet subscribed to by the MiR!" % sub_topic.topic)
+        
+        self.mir_bridge_ready = True
+        
 
     def get_topics(self):
         srv_response = self.robot.callService('/rosapi/topics', msg={})
@@ -438,6 +446,13 @@ def get_topics(self):
                 print((' * %s [%s]' % (topic_name, topic_type)))
 
         return topics
+    
+    def mir_bridge_ready_poll_callback(self, request, response):
+        self.get_logger().info('Checked for readiness')
+        response.success = self.mir_bridge_ready
+        response.message = ""
+        return response
+
 
 
 def main(args=None):
@@ -447,5 +462,6 @@ def main(args=None):
     rclpy.shutdown()
 
 
+
 if __name__ == '__main__':
     main()