Add emergency halt publisher

mir_driver/mir_driver/mir_bridge.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 import rclpy
 from rclpy.node import Node
-from rclpy.qos import qos_profile_system_default, qos_profile_sensor_data
+from rclpy.qos import qos_profile_system_default, qos_profile_sensor_data, qos_profile_services_default
 
 import time
 import copy
@@ -15,6 +15,7 @@
 from sensor_msgs.msg import LaserScan
 from tf2_msgs.msg import TFMessage
 from std_srvs.srv import Trigger
+from std_msgs.msg import Bool
 
 tf_prefix = ''
 
@@ -454,8 +455,42 @@ 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)
-
+
+
+        emergency_pub = self.create_publisher(Bool, 'emergency_halt', qos_profile_services_default)
+        cli_mir_restapi_emergency = self.create_client(Trigger, 'mir_100_is_emergency_halt')
+        cli_mir_restapi_emergency.wait_for_service()
         self.mir_bridge_ready = True
+
+        last_check = time.time()
+        emergency = False
+        while True:
+            rclpy.spin_once(self)
+            if time.time() - last_check < 1:
+                continue
+            last_check = time.time()
+
+            req = Trigger.Request()
+            future = cli_mir_restapi_emergency.call_async(req)
+            rclpy.spin_until_future_complete(self, future, timeout_sec=1)
+            if future.done():
+                if future.result().message == str(True):
+                    if not emergency:
+                        msg = Bool()
+                        msg.data = True
+                        emergency_pub.publish(msg)
+                        self.get_logger().info('Emergency Halt triggered')
+                    emergency = True
+                else:
+                    if emergency:
+                        msg = Bool()
+                        msg.data = True
+                        emergency_pub.publish(msg)
+                        self.get_logger().info('Emergency Halt released')
+                    emergency = False
+
+
+
 
     def get_topics(self):
         srv_response = self.robot.callService('/rosapi/topics', msg={})

mir_restapi/mir_restapi/__init__.py

@@ -0,0 +1 @@
+

mir_restapi/mir_restapi/mir_restapi_lib.py

@@ -3,7 +3,6 @@
 import http.client
 from datetime import datetime
 
-
 class HttpConnection():
 
     def __init__(self, logger, address, auth, api_prefix):
@@ -79,7 +78,7 @@ def is_connected(self, print=True):
             self.http.connection.connect()
             self.http.connection.close()
             if print:
-                self.logger.info("REST API: Connected!")
+                self.logger.debug("REST API: Connected!")
         except Exception as e:
             if print:
                 self.logger.warn('REST API: Attempt to connect failed: ' + str(e))
@@ -92,7 +91,7 @@ def is_available(self):
             return False
         else:
             return True
-
+            
     def wait_for_available(self):
         while True:
             if self.is_connected(print=False):
@@ -111,6 +110,7 @@ def get_state_id(self):
         status = self.get_status()
         state_id = status["state_id"]
         return state_id
+
     """ Choices are: {3, 4, 11}, State: {Ready, Pause, Manualcontrol}
     """
     def set_state_id(self, stateId):
@@ -241,7 +241,7 @@ def add_mission_to_queue(self, mission_name):
             self.logger.warn("Couldn't schedule mission")
             self.logger.warn(str(data))
         return False, -1
-
+        
     def is_mission_done(self, mission_queue_id):
         try:
             # mis_guid = self.get_mission_guid(mission_name)

mir_restapi/mir_restapi/mir_restapi_server.py

@@ -41,7 +41,7 @@ def setup_api_handle(self):
             self.get_logger().info("created MirRestAPI handle")
             self.create_services()
             self.get_logger().info("created services")
-
+            
     def parameters_callback(self, params):
         for param in params:
             if param.name == "mir_restapi_auth":
@@ -52,7 +52,7 @@ def parameters_callback(self, params):
                 self.hostname = param.value
         self.setup_api_handle()
         return SetParametersResult(successful=True)
-
+    
     def create_services(self):
         self.create_service(
             Trigger,
@@ -101,12 +101,12 @@ def create_services(self):
             'mir_100_get_settings',
             self.get_settings_callback)
         self.get_logger().info("Listening on 'mir_100_get_settings'")
-
+    
     def test_api_connection(self):
-        if self.api_handle is None:
+        if self.api_handle == None:
             return -1
-
-        self.get_logger().info('REST API: Waiting for connection')
+        
+        self.get_logger().debug('REST API: Waiting for connection')
         i = 1
         while not self.api_handle.is_connected():
             if not rclpy.ok():
@@ -117,13 +117,13 @@ def test_api_connection(self):
             i += 1
             time.sleep(1)
         return 1
-
+    
     def reponse_api_handle_not_exists(self, response):
         response.success = False
         response.message = 'API token and/or hostname not set yet'
         self.get_logger().error(response.message)
         return response
-
+    
     def call_restapi_function(self, service_fct, request, response, args=None):
         if self.test_api_connection() == -1:
             response = self.reponse_api_handle_not_exists(response)
@@ -153,33 +153,33 @@ def get_status_callback(self, request, response):
         self.get_logger().info('Getting status from REST API...')
         response = self.call_restapi_function(self.api_handle.get_status, request, response)
         return response
-
+    
     def get_sounds_callback(self, request, response):
         self.get_logger().info('Getting sounds from REST API...')
         response = self.call_restapi_function(self.api_handle.get_sounds, request, response)
         return response
-
+    
     def is_emergency_halt_callback(self, request, response):
-        self.get_logger().info('Checking REST API for emergency halt...')
+        self.get_logger().debug('Checking REST API for emergency halt...')
         response = self.call_restapi_function(self.api_handle.get_state_id, request, response)
-
+        
         if response.success:
             state_id = int(response.message)
-            #  self.get_logger().info("Returned state_id as %i" % state_id)
+            self.get_logger().debug("Returned state_id as %i" % state_id)
             STATE_ID_EMERGENCY = 10
             if state_id == STATE_ID_EMERGENCY:
                 response.message = str(True)
-                self.get_logger().info("Emergency Halt")
+                self.get_logger().debug("Emergency Halt")
             else:
                 response.message = str(False)
-                # self.get_logger().info("no emergency halt")
+                self.get_logger().debug("no emergency halt")
         return response
-
+    
     def get_missions_callback(self, request, response):
         self.get_logger().info('Getting missions from REST API...')
         response = self.call_restapi_function(self.api_handle.get_missions, request, response)
         return response
-
+    
     def honk_callback(self, request, response):
         self.get_logger().info('Honking horn over REST API...')
 
@@ -214,7 +214,7 @@ def honk_callback(self, request, response):
             self.api_handle.http.__del__()
             response.success = True
         return response
-
+    
     def get_system_info_callback(self, request, response):
         self.get_logger().info('Getting system info from REST API...')
         response = self.call_restapi_function(self.api_handle.get_system_info, request, response)

mir_restapi/mir_restapi/mir_restapi_sync_time.py

@@ -41,7 +41,7 @@ def call_trigger_service(self, client):
 
     def sync_time(self):
         self.call_trigger_service(self.restAPI_setTime)
-
+        
     def get_status(self):
         self.call_trigger_service(self.restAPI_getStatus)