diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index acf65fb7..08b8036e 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/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={}) diff --git a/mir_restapi/mir_restapi/__init__.py b/mir_restapi/mir_restapi/__init__.py index e69de29b..8b137891 100644 --- a/mir_restapi/mir_restapi/__init__.py +++ b/mir_restapi/mir_restapi/__init__.py @@ -0,0 +1 @@ + diff --git a/mir_restapi/mir_restapi/mir_restapi_lib.py b/mir_restapi/mir_restapi/mir_restapi_lib.py index eab4f6d5..1407ef55 100755 --- a/mir_restapi/mir_restapi/mir_restapi_lib.py +++ b/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) diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index 21f3823b..878e655d 100755 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/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) diff --git a/mir_restapi/mir_restapi/mir_restapi_sync_time.py b/mir_restapi/mir_restapi/mir_restapi_sync_time.py index 65f72180..b0a0114e 100644 --- a/mir_restapi/mir_restapi/mir_restapi_sync_time.py +++ b/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)