From 9279d6ccddc7d83346770e7d28489ebbee5f1c61 Mon Sep 17 00:00:00 2001 From: niemsoen Date: Thu, 30 Sep 2021 12:52:47 +0200 Subject: [PATCH 01/75] initial commit: added restapi state of work --- mir_driver/mir_driver/mir_bridge.py | 9 +- mir_restapi/LICENSE | 29 +++ mir_restapi/README.md | 2 + mir_restapi/action/mir100_setTime.action | 5 + mir_restapi/mir_restapi/__init__.py | 1 + mir_restapi/mir_restapi/mir_restapi_client.py | 56 +++++ mir_restapi/mir_restapi/mir_restapi_lib.py | 197 ++++++++++++++++++ mir_restapi/mir_restapi/mir_restapi_server.py | 83 ++++++++ mir_restapi/package.xml | 18 ++ mir_restapi/resources/mir_restapi | 1 + mir_restapi/setup.cfg | 4 + mir_restapi/setup.py | 27 +++ mir_restapi/test/test_copyright.py | 23 ++ mir_restapi/test/test_flake8.py | 25 +++ mir_restapi/test/test_pep257.py | 23 ++ 15 files changed, 502 insertions(+), 1 deletion(-) create mode 100644 mir_restapi/LICENSE create mode 100644 mir_restapi/README.md create mode 100644 mir_restapi/action/mir100_setTime.action create mode 100644 mir_restapi/mir_restapi/__init__.py create mode 100644 mir_restapi/mir_restapi/mir_restapi_client.py create mode 100644 mir_restapi/mir_restapi/mir_restapi_lib.py create mode 100644 mir_restapi/mir_restapi/mir_restapi_server.py create mode 100644 mir_restapi/package.xml create mode 100644 mir_restapi/resources/mir_restapi create mode 100644 mir_restapi/setup.cfg create mode 100644 mir_restapi/setup.py create mode 100644 mir_restapi/test/test_copyright.py create mode 100644 mir_restapi/test/test_flake8.py create mode 100644 mir_restapi/test/test_pep257.py diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index 540d4239..a0cdb092 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -11,6 +11,7 @@ from collections.abc import Iterable import mir_driver.rosbridge +import mir_driver.mir_restapi from rclpy_message_converter import message_converter from geometry_msgs.msg import TwistStamped from nav_msgs.msg import Odometry @@ -375,7 +376,13 @@ def __init__(self): global tf_prefix tf_prefix = self.declare_parameter('~tf_prefix', '').value.strip('/') - self.get_logger().info('Trying to connect to %s:%i...' % (hostname, port)) + + # Connect to Mir REST API: set date time + # call ROS action server here if needed + + + # Connect to ROSbridge + self.get_logger().info('Trying to connect to ROSbridge at %s:%i...' % (hostname, port)) self.robot = mir_driver.rosbridge.RosbridgeSetup(hostname, port) i = 1 diff --git a/mir_restapi/LICENSE b/mir_restapi/LICENSE new file mode 100644 index 00000000..41194b93 --- /dev/null +++ b/mir_restapi/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2021, niemsoen +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/mir_restapi/README.md b/mir_restapi/README.md new file mode 100644 index 00000000..20848071 --- /dev/null +++ b/mir_restapi/README.md @@ -0,0 +1,2 @@ +# mir_restapi +ROS action server that implements calls to the Mir REST API diff --git a/mir_restapi/action/mir100_setTime.action b/mir_restapi/action/mir100_setTime.action new file mode 100644 index 00000000..859b28f3 --- /dev/null +++ b/mir_restapi/action/mir100_setTime.action @@ -0,0 +1,5 @@ +int32 options +--- +boolean setTime +--- +boolean settingTime \ No newline at end of file diff --git a/mir_restapi/mir_restapi/__init__.py b/mir_restapi/mir_restapi/__init__.py new file mode 100644 index 00000000..8b137891 --- /dev/null +++ b/mir_restapi/mir_restapi/__init__.py @@ -0,0 +1 @@ + diff --git a/mir_restapi/mir_restapi/mir_restapi_client.py b/mir_restapi/mir_restapi/mir_restapi_client.py new file mode 100644 index 00000000..09659e34 --- /dev/null +++ b/mir_restapi/mir_restapi/mir_restapi_client.py @@ -0,0 +1,56 @@ +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node + +from action_tutorials_interfaces.action import Fibonacci + + +class FibonacciActionClient(Node): + + def __init__(self): + super().__init__('fibonacci_action_client') + self._action_client = ActionClient(self, Fibonacci, 'fibonacci') + + def send_goal(self, order): + goal_msg = Fibonacci.Goal() + goal_msg.order = order + + self._action_client.wait_for_server() + + self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback) + + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info('Goal rejected :(') + return + + self.get_logger().info('Goal accepted :)') + + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.get_result_callback) + + def get_result_callback(self, future): + result = future.result().result + self.get_logger().info('Result: {0}'.format(result.sequence)) + rclpy.shutdown() + + def feedback_callback(self, feedback_msg): + feedback = feedback_msg.feedback + self.get_logger().info('Received feedback: {0}'.format(feedback.partial_sequence)) + + +def main(args=None): + rclpy.init(args=args) + + action_client = FibonacciActionClient() + + action_client.send_goal(10) + + rclpy.spin(action_client) + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/mir_restapi/mir_restapi/mir_restapi_lib.py b/mir_restapi/mir_restapi/mir_restapi_lib.py new file mode 100644 index 00000000..5ba67710 --- /dev/null +++ b/mir_restapi/mir_restapi/mir_restapi_lib.py @@ -0,0 +1,197 @@ +import json +import time +import http.client +from datetime import datetime +import pytz + +class HttpConnection(): + + def __init__(self, logger, address, auth, api_prefix): + self.logger = logger + self.api_prefix = api_prefix + self.http_headers = { + "Accept-Language":"en-EN", + "Authorization":auth, + "Content-Type":"application/json"} + try: + self.connection = http.client.HTTPConnection(host=address, timeout=5) + except Exception as e: + self.logger.warn('Creation of http connection failed') + self.logger.warn(str(e)) + + def __del__(self): + if self.isValid(): + self.connection.close() + + def isValid(self): + return not self.connection is None + + def get(self, path): + if not self.isValid(): + self.connection.connect() + self.connection.request("GET", self.api_prefix+path, headers = self.http_headers) + resp = self.connection.getresponse() + if resp.status <200 or resp.status>=300: + self.logger.warn("GET failed with status {} and reason: {}".format(resp.status, resp.reason)) + return resp + + def post(self, path, body): + self.connection.request("POST", self.api_prefix+path,body=body, headers=self.http_headers) + resp = self.connection.getresponse() + if resp.status <200 or resp.status>=300: + self.logger.warn("POST failed with status {} and reason: {}".format(resp.status, resp.reason)) + return json.loads(resp.read()) + + def put(self, path, body): + self.connection.request("PUT", self.api_prefix+path,body=body, headers=self.http_headers) + resp = self.connection.getresponse() + #self.logger.info(resp.read()) + if resp.status <200 or resp.status>=300: + self.logger.warn("POST failed with status {} and reason: {}".format(resp.status, resp.reason)) + return json.loads(resp.read()) + + def putNoResponse(self, path, body): + self.connection.request("PUT", self.api_prefix+path,body=body, headers=self.http_headers) + +class MirRestAPI(): + + def __init__(self, logger, hostname, auth = ""): + self.logger = logger + if hostname is not None: + address = hostname + ":80" + else: + address="192.168.12.20:80" + self.http = HttpConnection(logger, address, auth, "/api/v2.0.0") + + def close(self): + self.http.__del__() + self.logger.info("REST API: Connection closed") + + def isConnected(self, printSuccess=True): + if not self.http.isValid(): + self.logger.warn('REST API: Http-Connection is not valid') + return False + try: + self.http.connection.connect() + self.http.connection.close() + except Exception as e: + self.logger.warn('REST API: Attempt to connect failed: ' + str(e)) + return False + if printSuccess: + self.logger.info("REST API: Connected!") + return True + + def getStatus(self): + response = self.http.get("/status") + return json.loads(response.read()) + + def getStateId(self): + status = self.getStatus() + state_id = status["state_id"] + return state_id + + """ Choices are: {3, 4, 11}, State: {Ready, Pause, Manualcontrol} + """ + def setStateId(self, stateId): + return self.http.put("/status", json.dumps({'state_id': stateId})) + + def isReady(self): + status = self.getStatus() + if status["state_id"] != 3: # 3=Ready, 4=Pause, 11=Manualcontrol + self.logger.warn("MIR currently occupied. System state: {}".format(status["state_text"])) + return False + else: + return True + + def getAllSettings(self, advanced=False, listGroups=False): + if advanced: + response = self.http.get("/settings/advanced") + elif listGroups: + response = self.http.get("/setting_groups") + else: + response = self.http.get("/settings") + return json.loads(response.read()) + + def getGroupSettings(self, groupID): + response = self.http.get("/setting_groups/" + groupID + "/settings") + return json.loads(response.read()) + + def setSetting(self, settingID, settingData): + return self.http.put("/setting", json.dumps({settingID: settingData})) + + def setDateTime(self): + timezone = pytz.timezone("Europe/Berlin") + timeobj = datetime.now(timezone) + self.logger.info("REST API: Set Timezone to Europe/Berlin") + + dT = timeobj.strftime("%Y-%m-%dT%X") + response = 'REST API: ' + try: + response = str(self.http.put("/status", json.dumps({'datetime': dT}))) + except Exception as e: + if str(e) == "timed out": + # setting datetime over REST API seems not to be intended + # that's why there is no response accompanying the PUT request, + # therefore a time out occurs, however time has been set correctly + response += "Set datetime" + else: + response = "Error setting datetime" + response = response + " to " + dT + + self.logger.info(response) + self.logger.info("REST API: Setting time Mir triggers emergency stop, please unlock.") + self.logger.info('REST API: Waiting to close connection: 10s') + time.sleep(5) # this is needed, so that the connection timeout can be ignored + self.logger.info('REST API: Waiting to close connection: 5s') + time.sleep(5) + + def getDistanceStatistics(self): + response = self.http.get("/statistics/distance") + return json.loads(response.read()) + + def getPositions(self): + response = self.http.get("/positions") + return json.loads(response.read()) + + def getPoseGuid(self, pos_name): + positions = self.getPositions() + return next((pos["guid"] for pos in positions if pos["name"]==pos_name), None) + + def getMissions(self): + response = self.http.get("/missions") + return json.loads(response.read()) + + def getMissionGuid(self, mission_name): + missions = self.getMissions() + return next((mis["guid"] for mis in missions if mis["name"]==mission_name), None) + + def moveTo(self, position, mission="MoveTo"): + mis_guid = self.getMissionGuid(mission) + pos_guid = self.getPoseGuid(position) + + for (var, txt, name) in zip((mis_guid, pos_guid),("Mission", "Position"),(mission, position)): + if var is None: + self.logger.warn("No {} named {} available on MIR - Aborting MoveTo".format(txt,name)) + return + + body = json.dumps({ + "mission_id": mis_guid, + "message": "Externally scheduled mission from the MIR Python Client", + "parameters": [{ + "value": pos_guid, + "input_name": "target" + }]}) + + data = self.http.post("/mission_queue", body) + self.logger.info("Mission scheduled for execution under id {}".format(data["id"])) + + while data["state"] != "Done": + resp = self.http.get("/mission_queue/{}".format(data["id"])) + data = json.loads(resp.read()) + if data["state"] == "Error": + self.logger.warn("Mission failed as robot is in error") + return + self.logger.info(data["state"]) + time.sleep(2) + + self.logger.info("Mission executed successfully") \ No newline at end of file diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py new file mode 100644 index 00000000..1f662079 --- /dev/null +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -0,0 +1,83 @@ +import time +import sys + +import rclpy +from rclpy.action import ActionServer +from rclpy.node import Node + +import mir_restapi.mir_restapi_lib +import mir_restapi.action + + +class MirRestAPIActionServer(Node): + + def __init__(self, hostname, auth): + super().__init__('mir_restapi') + + self.api_handle = mir_restapi.mir_restapi_lib.MirRestAPI( + self.get_logger(), hostname, auth) + + # === + # Action server definitions + # === + self.mir100_setTime_action_server = ActionServer( + self, + mir_restapi.action.mir100_setTime, + 'mir100_setTime', + self.mir100_setTime) + + def connectRESTapi(self): + self.get_logger().info('REST API: Waiting for connection') + i = 1 + while not self.api_handle.isConnected(): + if not rclpy.ok(): + sys.exit(0) + if i > 5: + self.get_logger().error('REST API: Could not connect, giving up') + break + i += 1 + time.sleep(1) + + + # === + # Action server callbacks + # === + + def mir100_setTime(self, goal_handle): + self.get_logger().info('Executing goal...') + # Feedback + feedback_msg = mir_restapi.action.mir100_setTime.Feedback() + feedback_msg.settingTime = "true" + self.get_logger().info('Feedback: {}'.format(feedback_msg.settingTime)) + goal_handle.publish_feedback(feedback_msg) + + self.connectRESTapi() + + # options: + # goal_handle.request.options + + # Request + if self.api_handle.isConnected(printSuccess=False): + self.api_handle.setDateTime() # produces an unavoidable connection timeout + self.api_handle.close() + + goal_handle.succeed() + + # Result + result = mir_restapi.action.mir100_setTime.Result() + result.setTime = "true" + return result + + +def main(args=None): + rclpy.init(args=args) + + hostname = "192.168.12.20" + auth = "" + mir_restapi_server = MirRestAPIActionServer(hostname, auth) + + rclpy.spin(mir_restapi_server) + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/mir_restapi/package.xml b/mir_restapi/package.xml new file mode 100644 index 00000000..49c9f97d --- /dev/null +++ b/mir_restapi/package.xml @@ -0,0 +1,18 @@ + + + + mir_restapi + 0.0.0 + TODO: Package description + niemann + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/mir_restapi/resources/mir_restapi b/mir_restapi/resources/mir_restapi new file mode 100644 index 00000000..8b137891 --- /dev/null +++ b/mir_restapi/resources/mir_restapi @@ -0,0 +1 @@ + diff --git a/mir_restapi/setup.cfg b/mir_restapi/setup.cfg new file mode 100644 index 00000000..a9845014 --- /dev/null +++ b/mir_restapi/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/mir_restapi +[install] +install-scripts=$base/lib/mir_restapi diff --git a/mir_restapi/setup.py b/mir_restapi/setup.py new file mode 100644 index 00000000..de5f6a00 --- /dev/null +++ b/mir_restapi/setup.py @@ -0,0 +1,27 @@ +from setuptools import setup + +package_name = 'mir_restapi' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='niemann', + maintainer_email='soenke.niemann@ipk.fraunhofer.de', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'mir_restapi_server = mir_restapi.mir_restapi_server:main', + 'mir_restapi_client = mir_restapi.mir_restapi_client:main' + ], + }, +) diff --git a/mir_restapi/test/test_copyright.py b/mir_restapi/test/test_copyright.py new file mode 100644 index 00000000..cc8ff03f --- /dev/null +++ b/mir_restapi/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/mir_restapi/test/test_flake8.py b/mir_restapi/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/mir_restapi/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/mir_restapi/test/test_pep257.py b/mir_restapi/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/mir_restapi/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' From 2a0dde8e64347830e5d022b0e38711b3db412b66 Mon Sep 17 00:00:00 2001 From: niemsoen Date: Thu, 14 Oct 2021 15:57:48 +0200 Subject: [PATCH 02/75] added mir_ready publisher --- mir_driver/mir_driver/mir_bridge.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index 540d4239..d90da257 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/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_msgs.msg import Bool tf_prefix = '' @@ -372,6 +373,13 @@ def __init__(self): port = self.declare_parameter('port', 9090).value assert isinstance(port, int), 'port parameter must be an integer' + # publisher that signifies that mir_bridge is ready + # default to False + pub_mir_ready = self.create_publisher(Bool, "mir_bridge_ready", 10) + msg_mir_ready = Bool() + msg_mir_ready.data = False + pub_mir_ready.publish(msg_mir_ready) + global tf_prefix tf_prefix = self.declare_parameter('~tf_prefix', '').value.strip('/') @@ -409,6 +417,10 @@ 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) + + # signal to button that mir_bridge is ready + msg_mir_ready.data = True + pub_mir_ready.publish(msg_mir_ready) def get_topics(self): srv_response = self.robot.callService('/rosapi/topics', msg={}) From 7cf090381af9ce6c66adbfaa4b14199322caacd8 Mon Sep 17 00:00:00 2001 From: niemsoen Date: Fri, 15 Oct 2021 10:57:05 +0200 Subject: [PATCH 03/75] added reliable QOS for mir_bridge_ready topic --- mir_driver/mir_driver/mir_bridge.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index d90da257..367e40a7 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -17,6 +17,7 @@ from sensor_msgs.msg import LaserScan from tf2_msgs.msg import TFMessage from std_msgs.msg import Bool +from rclpy.qos import qos_profile_services_default tf_prefix = '' @@ -375,7 +376,7 @@ def __init__(self): # publisher that signifies that mir_bridge is ready # default to False - pub_mir_ready = self.create_publisher(Bool, "mir_bridge_ready", 10) + pub_mir_ready = self.create_publisher(Bool, "mir_bridge_ready", qos_profile_services_default) msg_mir_ready = Bool() msg_mir_ready.data = False pub_mir_ready.publish(msg_mir_ready) From 4fe0c37b0539f4df807e85f3767f5f3423a7cd05 Mon Sep 17 00:00:00 2001 From: niemsoen Date: Fri, 15 Oct 2021 12:36:31 +0200 Subject: [PATCH 04/75] added mir_bridge_ready=false on crash/shutdown --- mir_driver/mir_driver/mir_bridge.py | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index 367e40a7..0ea18ece 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -456,8 +456,17 @@ def get_topics(self): def main(args=None): rclpy.init(args=args) node = MiR100BridgeNode() - rclpy.spin(node) - rclpy.shutdown() + try: + rclpy.spin(node) + except Exception as e: + node.get_logger().fatal(str(e)) + finally: + # send not ready signal on crash/shutdown + msg_mir_ready = Bool() + msg_mir_ready.data = False + node.pub_mir_ready.publish(msg_mir_ready) + rclpy.shutdown() + if __name__ == '__main__': From 02ee545064add171f8b2ac2c26880cc1377cf74b Mon Sep 17 00:00:00 2001 From: niemsoen Date: Fri, 15 Oct 2021 12:51:07 +0200 Subject: [PATCH 05/75] attribute bugfix --- mir_driver/mir_driver/mir_bridge.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index 0ea18ece..ac2f9885 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -376,10 +376,10 @@ def __init__(self): # publisher that signifies that mir_bridge is ready # default to False - pub_mir_ready = self.create_publisher(Bool, "mir_bridge_ready", qos_profile_services_default) + self.pub_mir_ready = self.create_publisher(Bool, "mir_bridge_ready", qos_profile_services_default) msg_mir_ready = Bool() msg_mir_ready.data = False - pub_mir_ready.publish(msg_mir_ready) + self.pub_mir_ready.publish(msg_mir_ready) global tf_prefix tf_prefix = self.declare_parameter('~tf_prefix', '').value.strip('/') @@ -421,7 +421,7 @@ def __init__(self): # signal to button that mir_bridge is ready msg_mir_ready.data = True - pub_mir_ready.publish(msg_mir_ready) + self.pub_mir_ready.publish(msg_mir_ready) def get_topics(self): srv_response = self.robot.callService('/rosapi/topics', msg={}) From 137c7a599fe9bc16a6e05ea323b35e99dfaebe99 Mon Sep 17 00:00:00 2001 From: niemsoen Date: Fri, 15 Oct 2021 15:06:15 +0200 Subject: [PATCH 06/75] added readiness polling and simplified ready publishing --- mir_driver/mir_driver/mir_bridge.py | 32 ++++++++++++++++++----------- 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index ac2f9885..0872b7b0 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -366,6 +366,15 @@ def callback(self, msg): class MiR100BridgeNode(Node): def __init__(self): super().__init__('mir_bridge') + + # publisher that signifies that mir_bridge is ready + self.pub_mir_ready = self.create_publisher(Bool, "mir_bridge_ready", qos_profile_services_default) + self.mir_bridge_ready = False + self.publish_mir_ready_state(self.mir_bridge_ready) + # listener that answers to polling of mir_bridge readiness + self.sub_mir_ready_poll = self.create_subscription(Bool, "mir_bridge_ready_poll", + self.mir_bridge_ready_poll_callback, qos_profile_services_default) + try: hostname = self.declare_parameter('hostname', '192.168.12.20').value except KeyError: @@ -374,13 +383,6 @@ def __init__(self): port = self.declare_parameter('port', 9090).value assert isinstance(port, int), 'port parameter must be an integer' - # publisher that signifies that mir_bridge is ready - # default to False - self.pub_mir_ready = self.create_publisher(Bool, "mir_bridge_ready", qos_profile_services_default) - msg_mir_ready = Bool() - msg_mir_ready.data = False - self.pub_mir_ready.publish(msg_mir_ready) - global tf_prefix tf_prefix = self.declare_parameter('~tf_prefix', '').value.strip('/') @@ -420,8 +422,7 @@ def __init__(self): "Topic '%s' is not yet subscribed to by the MiR!" % sub_topic.topic) # signal to button that mir_bridge is ready - msg_mir_ready.data = True - self.pub_mir_ready.publish(msg_mir_ready) + self.publish_mir_ready_state(True) def get_topics(self): srv_response = self.robot.callService('/rosapi/topics', msg={}) @@ -451,6 +452,15 @@ def get_topics(self): print((' * %s [%s]' % (topic_name, topic_type))) return topics + + def mir_bridge_ready_poll_callback(self, msg): + self.publish_mir_ready_state(self.mir_bridge_ready) + + def publish_mir_ready_state(self, state): + self.mir_bridge_ready = state + msg_mir_ready = Bool() + msg_mir_ready.data = self.mir_bridge_ready + self.pub_mir_ready.publish(msg_mir_ready) def main(args=None): @@ -462,9 +472,7 @@ def main(args=None): node.get_logger().fatal(str(e)) finally: # send not ready signal on crash/shutdown - msg_mir_ready = Bool() - msg_mir_ready.data = False - node.pub_mir_ready.publish(msg_mir_ready) + node.publish_mir_ready_state(False) rclpy.shutdown() From f2d0e04db8f9ae1e5c2dfedaff6712c95a699b9e Mon Sep 17 00:00:00 2001 From: niemsoen Date: Mon, 18 Oct 2021 11:47:08 +0200 Subject: [PATCH 07/75] uneccesary main->publish_mir_ready_state: button checks bridge alive --- mir_driver/mir_driver/mir_bridge.py | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index 0872b7b0..f9300cc8 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -466,14 +466,8 @@ def publish_mir_ready_state(self, state): def main(args=None): rclpy.init(args=args) node = MiR100BridgeNode() - try: - rclpy.spin(node) - except Exception as e: - node.get_logger().fatal(str(e)) - finally: - # send not ready signal on crash/shutdown - node.publish_mir_ready_state(False) - rclpy.shutdown() + rclpy.spin(node) + rclpy.shutdown() From ab51022a717855363c62208855b974ad5aeac141 Mon Sep 17 00:00:00 2001 From: niemsoen Date: Mon, 18 Oct 2021 11:47:08 +0200 Subject: [PATCH 08/75] uneccesary main->publish_mir_ready_state: button checks bridge alive --- mir_driver/mir_driver/mir_bridge.py | 27 ++++++++++++++------------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index 0872b7b0..5be65e08 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -368,12 +368,19 @@ def __init__(self): super().__init__('mir_bridge') # publisher that signifies that mir_bridge is ready - self.pub_mir_ready = self.create_publisher(Bool, "mir_bridge_ready", qos_profile_services_default) - self.mir_bridge_ready = False - self.publish_mir_ready_state(self.mir_bridge_ready) + self.pub_mir_ready = self.create_publisher( + Bool, + "mir_bridge_ready", + qos_profile_services_default) + # mir_bridge_ready state variable + self.mir_bridge_ready = None + self.publish_mir_ready_state(False) # listener that answers to polling of mir_bridge readiness - self.sub_mir_ready_poll = self.create_subscription(Bool, "mir_bridge_ready_poll", - self.mir_bridge_ready_poll_callback, qos_profile_services_default) + self.sub_mir_ready_poll = self.create_subscription( + Bool, + "mir_bridge_ready_poll", + self.mir_bridge_ready_poll_callback, + qos_profile_services_default) try: hostname = self.declare_parameter('hostname', '192.168.12.20').value @@ -466,14 +473,8 @@ def publish_mir_ready_state(self, state): def main(args=None): rclpy.init(args=args) node = MiR100BridgeNode() - try: - rclpy.spin(node) - except Exception as e: - node.get_logger().fatal(str(e)) - finally: - # send not ready signal on crash/shutdown - node.publish_mir_ready_state(False) - rclpy.shutdown() + rclpy.spin(node) + rclpy.shutdown() From 1ba188cdbe0d44e212c3920fb903717537d6235a Mon Sep 17 00:00:00 2001 From: niemsoen Date: Mon, 18 Oct 2021 13:52:16 +0200 Subject: [PATCH 09/75] changed mir_ready pub/sub to Trigger-service --- mir_driver/mir_driver/mir_bridge.py | 40 ++++++++++------------------- 1 file changed, 14 insertions(+), 26 deletions(-) diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index 5be65e08..45a40bd4 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -16,8 +16,7 @@ from nav_msgs.msg import Odometry from sensor_msgs.msg import LaserScan from tf2_msgs.msg import TFMessage -from std_msgs.msg import Bool -from rclpy.qos import qos_profile_services_default +from std_srvs.srv import Trigger tf_prefix = '' @@ -367,20 +366,11 @@ class MiR100BridgeNode(Node): def __init__(self): super().__init__('mir_bridge') - # publisher that signifies that mir_bridge is ready - self.pub_mir_ready = self.create_publisher( - Bool, - "mir_bridge_ready", - qos_profile_services_default) - # mir_bridge_ready state variable - self.mir_bridge_ready = None - self.publish_mir_ready_state(False) - # listener that answers to polling of mir_bridge readiness - self.sub_mir_ready_poll = self.create_subscription( - Bool, - "mir_bridge_ready_poll", - self.mir_bridge_ready_poll_callback, - qos_profile_services_default) + 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 @@ -428,8 +418,8 @@ def __init__(self): self.get_logger().warn( "Topic '%s' is not yet subscribed to by the MiR!" % sub_topic.topic) - # signal to button that mir_bridge is ready - self.publish_mir_ready_state(True) + self.mir_bridge_ready = True + def get_topics(self): srv_response = self.robot.callService('/rosapi/topics', msg={}) @@ -460,14 +450,12 @@ def get_topics(self): return topics - def mir_bridge_ready_poll_callback(self, msg): - self.publish_mir_ready_state(self.mir_bridge_ready) - - def publish_mir_ready_state(self, state): - self.mir_bridge_ready = state - msg_mir_ready = Bool() - msg_mir_ready.data = self.mir_bridge_ready - self.pub_mir_ready.publish(msg_mir_ready) + def mir_bridge_ready_poll_callback(self, response): + self.get_logger().info('Checked for readiness') + response.success = self.mir_bridge_ready + response.message = "" + return response + def main(args=None): From e91279ee30d4e2335331b6ce379db9f373838268 Mon Sep 17 00:00:00 2001 From: niemsoen Date: Tue, 19 Oct 2021 12:05:57 +0200 Subject: [PATCH 10/75] possible bugfix --- mir_driver/mir_driver/mir_bridge.py | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index 45a40bd4..00677be2 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -367,10 +367,7 @@ 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) + 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 @@ -450,7 +447,7 @@ def get_topics(self): return topics - def mir_bridge_ready_poll_callback(self, response): + def mir_bridge_ready_poll_callback(self, request, response): self.get_logger().info('Checked for readiness') response.success = self.mir_bridge_ready response.message = "" From f31efb898b94aac086fe88a3a940abc16fd3ff1d Mon Sep 17 00:00:00 2001 From: niemsoen Date: Wed, 20 Oct 2021 14:17:54 +0200 Subject: [PATCH 11/75] changed response to success=True always and message=True/False --- mir_driver/mir_driver/mir_bridge.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index 00677be2..1504eac7 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -449,8 +449,8 @@ def get_topics(self): def mir_bridge_ready_poll_callback(self, request, response): self.get_logger().info('Checked for readiness') - response.success = self.mir_bridge_ready - response.message = "" + response.success = True + response.message = str(self.mir_bridge_ready) return response From 6e00df4f4f2368faa1c7a30bd4b6e70f39ed5bf1 Mon Sep 17 00:00:00 2001 From: niemsoen Date: Wed, 20 Oct 2021 14:29:36 +0200 Subject: [PATCH 12/75] Revert "changed response to success=True always and message=True/False" This reverts commit f31efb898b94aac086fe88a3a940abc16fd3ff1d. --- mir_driver/mir_driver/mir_bridge.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index 1504eac7..00677be2 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -449,8 +449,8 @@ def get_topics(self): def mir_bridge_ready_poll_callback(self, request, response): self.get_logger().info('Checked for readiness') - response.success = True - response.message = str(self.mir_bridge_ready) + response.success = self.mir_bridge_ready + response.message = "" return response From c84ccb68a61ec11928fe0bf1aa7e7633bdee21bd Mon Sep 17 00:00:00 2001 From: niemsoen Date: Fri, 22 Oct 2021 11:18:54 +0200 Subject: [PATCH 13/75] added custom CheckReady srv-definition --- mir_driver/mir_driver/mir_bridge.py | 12 ++-- mir_driver/package.xml | 84 ++++++++++++------------ mir_driver_interfaces/CMakeLists.txt | 23 +++++++ mir_driver_interfaces/package.xml | 22 +++++++ mir_driver_interfaces/srv/CheckReady.srv | 4 ++ 5 files changed, 99 insertions(+), 46 deletions(-) create mode 100644 mir_driver_interfaces/CMakeLists.txt create mode 100644 mir_driver_interfaces/package.xml create mode 100644 mir_driver_interfaces/srv/CheckReady.srv diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index 00677be2..ed95efad 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -16,7 +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 +from mir_driver_interfaces.srv import CheckReady tf_prefix = '' @@ -367,7 +367,10 @@ 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) + self.srv_mir_ready = self.create_service( + CheckReady, + 'mir_bridge_ready', + self.mir_bridge_ready_poll_callback) try: hostname = self.declare_parameter('hostname', '192.168.12.20').value @@ -448,9 +451,8 @@ def get_topics(self): 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 = "" + response.success = True + response.ready = self.mir_bridge_ready return response diff --git a/mir_driver/package.xml b/mir_driver/package.xml index 8f66f6b2..8c01b7e7 100644 --- a/mir_driver/package.xml +++ b/mir_driver/package.xml @@ -1,41 +1,43 @@ - - - mir_driver - 1.1.3 - A reverse ROS2 bridge for the MiR100 robot - - Martin Günther - Martin Günther - - BSD - Apache 2.0 - - https://github.com/dfki-ric/mir_robot - https://github.com/dfki-ric/mir_robot - https://github.com/dfki-ric/mir_robot/issues - - geometry_msgs - mir_msgs - rclpy_message_converter - ira_laser_tools - nav_msgs - python3-websocket - rosgraph_msgs - rclpy - sensor_msgs - std_msgs - tf2_msgs - visualization_msgs - - twist_stamper - mir_description - robot_state_publisher - - ament_lint_auto - ament_lint_common - - - ament_python - - - + + + mir_driver + 1.1.3 + A reverse ROS2 bridge for the MiR100 robot + + Martin Günther + Martin Günther + + BSD + Apache 2.0 + + https://github.com/dfki-ric/mir_robot + https://github.com/dfki-ric/mir_robot + https://github.com/dfki-ric/mir_robot/issues + + geometry_msgs + mir_msgs + rclpy_message_converter + ira_laser_tools + nav_msgs + python3-websocket + rosgraph_msgs + rclpy + sensor_msgs + std_msgs + tf2_msgs + visualization_msgs + + mir_driver_interfaces + + twist_stamper + mir_description + robot_state_publisher + + ament_lint_auto + ament_lint_common + + + ament_python + + + diff --git a/mir_driver_interfaces/CMakeLists.txt b/mir_driver_interfaces/CMakeLists.txt new file mode 100644 index 00000000..167c7ca5 --- /dev/null +++ b/mir_driver_interfaces/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.8) +project(mir_driver_interfaces) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + + +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/CheckReady.srv" + ) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/mir_driver_interfaces/package.xml b/mir_driver_interfaces/package.xml new file mode 100644 index 00000000..a17b2bff --- /dev/null +++ b/mir_driver_interfaces/package.xml @@ -0,0 +1,22 @@ + + + + mir_driver_interfaces + 0.0.0 + TODO: Package description + koffsoph + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/mir_driver_interfaces/srv/CheckReady.srv b/mir_driver_interfaces/srv/CheckReady.srv new file mode 100644 index 00000000..e38555ad --- /dev/null +++ b/mir_driver_interfaces/srv/CheckReady.srv @@ -0,0 +1,4 @@ + +--- +bool success # indicate successful run of triggered service +bool ready # ready state of node's service it depends on \ No newline at end of file From 691f5742107410003f13cfdc784f72a7138c88ac Mon Sep 17 00:00:00 2001 From: niemsoen Date: Fri, 22 Oct 2021 11:54:58 +0200 Subject: [PATCH 14/75] find bug in service --- mir_driver/mir_driver/mir_bridge.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index ed95efad..8d859489 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -367,10 +367,7 @@ def __init__(self): super().__init__('mir_bridge') self.mir_bridge_ready = False #state - self.srv_mir_ready = self.create_service( - CheckReady, - 'mir_bridge_ready', - self.mir_bridge_ready_poll_callback) + self.srv_mir_ready = self.create_service(CheckReady,'mir_bridge_ready',self.mir_bridge_ready_poll_callback) try: hostname = self.declare_parameter('hostname', '192.168.12.20').value @@ -451,6 +448,7 @@ def get_topics(self): return topics def mir_bridge_ready_poll_callback(self, request, response): + self.get_logger().info("Called for mir_bridge_ready") response.success = True response.ready = self.mir_bridge_ready return response From 34cb67c53b48cc8a3ea729789e9f87b4188f504d Mon Sep 17 00:00:00 2001 From: niemsoen Date: Fri, 22 Oct 2021 12:07:33 +0200 Subject: [PATCH 15/75] renamed interface to match usb_pushbutton --- .../CMakeLists.txt | 2 +- .../package.xml | 2 +- .../srv/CheckReady.srv | 0 3 files changed, 2 insertions(+), 2 deletions(-) rename {mir_driver_interfaces => usb_pushbutton_interfaces}/CMakeLists.txt (93%) rename {mir_driver_interfaces => usb_pushbutton_interfaces}/package.xml (94%) rename {mir_driver_interfaces => usb_pushbutton_interfaces}/srv/CheckReady.srv (100%) diff --git a/mir_driver_interfaces/CMakeLists.txt b/usb_pushbutton_interfaces/CMakeLists.txt similarity index 93% rename from mir_driver_interfaces/CMakeLists.txt rename to usb_pushbutton_interfaces/CMakeLists.txt index 167c7ca5..a49875a5 100644 --- a/mir_driver_interfaces/CMakeLists.txt +++ b/usb_pushbutton_interfaces/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(mir_driver_interfaces) +project(usb_pushbutton_interfaces) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/mir_driver_interfaces/package.xml b/usb_pushbutton_interfaces/package.xml similarity index 94% rename from mir_driver_interfaces/package.xml rename to usb_pushbutton_interfaces/package.xml index a17b2bff..db710121 100644 --- a/mir_driver_interfaces/package.xml +++ b/usb_pushbutton_interfaces/package.xml @@ -1,7 +1,7 @@ - mir_driver_interfaces + usb_pushbutton_interfaces 0.0.0 TODO: Package description koffsoph diff --git a/mir_driver_interfaces/srv/CheckReady.srv b/usb_pushbutton_interfaces/srv/CheckReady.srv similarity index 100% rename from mir_driver_interfaces/srv/CheckReady.srv rename to usb_pushbutton_interfaces/srv/CheckReady.srv From eb07640e19e4142bd9a7992dcf5143da574a97ed Mon Sep 17 00:00:00 2001 From: niemsoen Date: Fri, 22 Oct 2021 12:13:03 +0200 Subject: [PATCH 16/75] fixed dependency --- mir_driver/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mir_driver/package.xml b/mir_driver/package.xml index 8c01b7e7..ba16aee3 100644 --- a/mir_driver/package.xml +++ b/mir_driver/package.xml @@ -27,7 +27,7 @@ tf2_msgs visualization_msgs - mir_driver_interfaces + usb_pushbutton_interfaces twist_stamper mir_description From 28b8218e288537c63d02f6edc7f69c929f229402 Mon Sep 17 00:00:00 2001 From: niemsoen Date: Fri, 22 Oct 2021 12:14:49 +0200 Subject: [PATCH 17/75] fixed another dependency --- mir_driver/mir_driver/mir_bridge.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index 8d859489..d389a8b3 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -16,7 +16,7 @@ from nav_msgs.msg import Odometry from sensor_msgs.msg import LaserScan from tf2_msgs.msg import TFMessage -from mir_driver_interfaces.srv import CheckReady +from usb_pushbutton_interfaces.srv import CheckReady tf_prefix = '' From 69de4f73afba14852639dac2e009dc9cf48a7472 Mon Sep 17 00:00:00 2001 From: "soenke.niemann" Date: Wed, 10 Nov 2021 18:55:54 +0100 Subject: [PATCH 18/75] Revert "Merge pull request #2 from niemsoen/mir-bridge-as-service-server" This reverts commit cf0f5c18334f74c283a3214178d6d45c6636e570, reversing changes made to 39e025e15eef8591a962283291e1a7fe37fc285e. --- mir_driver/mir_driver/mir_bridge.py | 37 ++++++--- mir_driver/package.xml | 84 ++++++++++---------- usb_pushbutton_interfaces/CMakeLists.txt | 23 ------ usb_pushbutton_interfaces/package.xml | 22 ----- usb_pushbutton_interfaces/srv/CheckReady.srv | 4 - 5 files changed, 67 insertions(+), 103 deletions(-) delete mode 100644 usb_pushbutton_interfaces/CMakeLists.txt delete mode 100644 usb_pushbutton_interfaces/package.xml delete mode 100644 usb_pushbutton_interfaces/srv/CheckReady.srv diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index d389a8b3..5be65e08 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -16,7 +16,8 @@ from nav_msgs.msg import Odometry from sensor_msgs.msg import LaserScan from tf2_msgs.msg import TFMessage -from usb_pushbutton_interfaces.srv import CheckReady +from std_msgs.msg import Bool +from rclpy.qos import qos_profile_services_default tf_prefix = '' @@ -366,8 +367,20 @@ class MiR100BridgeNode(Node): def __init__(self): super().__init__('mir_bridge') - self.mir_bridge_ready = False #state - self.srv_mir_ready = self.create_service(CheckReady,'mir_bridge_ready',self.mir_bridge_ready_poll_callback) + # publisher that signifies that mir_bridge is ready + self.pub_mir_ready = self.create_publisher( + Bool, + "mir_bridge_ready", + qos_profile_services_default) + # mir_bridge_ready state variable + self.mir_bridge_ready = None + self.publish_mir_ready_state(False) + # listener that answers to polling of mir_bridge readiness + self.sub_mir_ready_poll = self.create_subscription( + Bool, + "mir_bridge_ready_poll", + self.mir_bridge_ready_poll_callback, + qos_profile_services_default) try: hostname = self.declare_parameter('hostname', '192.168.12.20').value @@ -415,8 +428,8 @@ def __init__(self): self.get_logger().warn( "Topic '%s' is not yet subscribed to by the MiR!" % sub_topic.topic) - self.mir_bridge_ready = True - + # signal to button that mir_bridge is ready + self.publish_mir_ready_state(True) def get_topics(self): srv_response = self.robot.callService('/rosapi/topics', msg={}) @@ -447,12 +460,14 @@ def get_topics(self): return topics - def mir_bridge_ready_poll_callback(self, request, response): - self.get_logger().info("Called for mir_bridge_ready") - response.success = True - response.ready = self.mir_bridge_ready - return response - + def mir_bridge_ready_poll_callback(self, msg): + self.publish_mir_ready_state(self.mir_bridge_ready) + + def publish_mir_ready_state(self, state): + self.mir_bridge_ready = state + msg_mir_ready = Bool() + msg_mir_ready.data = self.mir_bridge_ready + self.pub_mir_ready.publish(msg_mir_ready) def main(args=None): diff --git a/mir_driver/package.xml b/mir_driver/package.xml index ba16aee3..8f66f6b2 100644 --- a/mir_driver/package.xml +++ b/mir_driver/package.xml @@ -1,43 +1,41 @@ - - - mir_driver - 1.1.3 - A reverse ROS2 bridge for the MiR100 robot - - Martin Günther - Martin Günther - - BSD - Apache 2.0 - - https://github.com/dfki-ric/mir_robot - https://github.com/dfki-ric/mir_robot - https://github.com/dfki-ric/mir_robot/issues - - geometry_msgs - mir_msgs - rclpy_message_converter - ira_laser_tools - nav_msgs - python3-websocket - rosgraph_msgs - rclpy - sensor_msgs - std_msgs - tf2_msgs - visualization_msgs - - usb_pushbutton_interfaces - - twist_stamper - mir_description - robot_state_publisher - - ament_lint_auto - ament_lint_common - - - ament_python - - - + + + mir_driver + 1.1.3 + A reverse ROS2 bridge for the MiR100 robot + + Martin Günther + Martin Günther + + BSD + Apache 2.0 + + https://github.com/dfki-ric/mir_robot + https://github.com/dfki-ric/mir_robot + https://github.com/dfki-ric/mir_robot/issues + + geometry_msgs + mir_msgs + rclpy_message_converter + ira_laser_tools + nav_msgs + python3-websocket + rosgraph_msgs + rclpy + sensor_msgs + std_msgs + tf2_msgs + visualization_msgs + + twist_stamper + mir_description + robot_state_publisher + + ament_lint_auto + ament_lint_common + + + ament_python + + + diff --git a/usb_pushbutton_interfaces/CMakeLists.txt b/usb_pushbutton_interfaces/CMakeLists.txt deleted file mode 100644 index a49875a5..00000000 --- a/usb_pushbutton_interfaces/CMakeLists.txt +++ /dev/null @@ -1,23 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(usb_pushbutton_interfaces) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(rosidl_default_generators REQUIRED) - - -rosidl_generate_interfaces(${PROJECT_NAME} - "srv/CheckReady.srv" - ) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/usb_pushbutton_interfaces/package.xml b/usb_pushbutton_interfaces/package.xml deleted file mode 100644 index db710121..00000000 --- a/usb_pushbutton_interfaces/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - usb_pushbutton_interfaces - 0.0.0 - TODO: Package description - koffsoph - TODO: License declaration - - ament_cmake - - ament_lint_auto - ament_lint_common - - rosidl_default_generators - rosidl_default_runtime - rosidl_interface_packages - - - ament_cmake - - diff --git a/usb_pushbutton_interfaces/srv/CheckReady.srv b/usb_pushbutton_interfaces/srv/CheckReady.srv deleted file mode 100644 index e38555ad..00000000 --- a/usb_pushbutton_interfaces/srv/CheckReady.srv +++ /dev/null @@ -1,4 +0,0 @@ - ---- -bool success # indicate successful run of triggered service -bool ready # ready state of node's service it depends on \ No newline at end of file From bb0eba0b3f7f84ceb14ca7910236c2a6275f83ef Mon Sep 17 00:00:00 2001 From: "soenke.niemann" Date: Wed, 10 Nov 2021 19:01:45 +0100 Subject: [PATCH 19/75] Use std_srvs/Trigger as mir-ready-service Type --- mir_driver/mir_driver/mir_bridge.py | 37 +++++++++-------------------- 1 file changed, 11 insertions(+), 26 deletions(-) diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index 5be65e08..00677be2 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -16,8 +16,7 @@ from nav_msgs.msg import Odometry from sensor_msgs.msg import LaserScan from tf2_msgs.msg import TFMessage -from std_msgs.msg import Bool -from rclpy.qos import qos_profile_services_default +from std_srvs.srv import Trigger tf_prefix = '' @@ -367,20 +366,8 @@ class MiR100BridgeNode(Node): def __init__(self): super().__init__('mir_bridge') - # publisher that signifies that mir_bridge is ready - self.pub_mir_ready = self.create_publisher( - Bool, - "mir_bridge_ready", - qos_profile_services_default) - # mir_bridge_ready state variable - self.mir_bridge_ready = None - self.publish_mir_ready_state(False) - # listener that answers to polling of mir_bridge readiness - self.sub_mir_ready_poll = self.create_subscription( - Bool, - "mir_bridge_ready_poll", - self.mir_bridge_ready_poll_callback, - qos_profile_services_default) + 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 @@ -428,8 +415,8 @@ def __init__(self): self.get_logger().warn( "Topic '%s' is not yet subscribed to by the MiR!" % sub_topic.topic) - # signal to button that mir_bridge is ready - self.publish_mir_ready_state(True) + self.mir_bridge_ready = True + def get_topics(self): srv_response = self.robot.callService('/rosapi/topics', msg={}) @@ -460,14 +447,12 @@ def get_topics(self): return topics - def mir_bridge_ready_poll_callback(self, msg): - self.publish_mir_ready_state(self.mir_bridge_ready) - - def publish_mir_ready_state(self, state): - self.mir_bridge_ready = state - msg_mir_ready = Bool() - msg_mir_ready.data = self.mir_bridge_ready - self.pub_mir_ready.publish(msg_mir_ready) + 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): From cbd49eef374ba925c3ecbd77fd963ea6363b2c69 Mon Sep 17 00:00:00 2001 From: niemsoen Date: Thu, 9 Dec 2021 11:05:25 +0100 Subject: [PATCH 20/75] changed structure to service-client communication --- mir_driver/mir_driver/mir_bridge.py | 2 +- mir_restapi/action/mir100_setTime.action | 5 -- mir_restapi/launch/mir_restapi_launch.py | 26 +++++++++ mir_restapi/mir_restapi/mir_restapi_client.py | 49 ++++------------- mir_restapi/mir_restapi/mir_restapi_server.py | 53 +++++++------------ 5 files changed, 56 insertions(+), 79 deletions(-) delete mode 100644 mir_restapi/action/mir100_setTime.action create mode 100644 mir_restapi/launch/mir_restapi_launch.py diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index 969633f4..4c932b17 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -11,7 +11,7 @@ from collections.abc import Iterable import mir_driver.rosbridge -import mir_driver.mir_restapi +import mir_restapi.mir_restapi from rclpy_message_converter import message_converter from geometry_msgs.msg import TwistStamped from nav_msgs.msg import Odometry diff --git a/mir_restapi/action/mir100_setTime.action b/mir_restapi/action/mir100_setTime.action deleted file mode 100644 index 859b28f3..00000000 --- a/mir_restapi/action/mir100_setTime.action +++ /dev/null @@ -1,5 +0,0 @@ -int32 options ---- -boolean setTime ---- -boolean settingTime \ No newline at end of file diff --git a/mir_restapi/launch/mir_restapi_launch.py b/mir_restapi/launch/mir_restapi_launch.py new file mode 100644 index 00000000..d3bb5518 --- /dev/null +++ b/mir_restapi/launch/mir_restapi_launch.py @@ -0,0 +1,26 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + + return LaunchDescription([ + + Node( + package='mir_restapi', + executable='mir_restapi_server', + output='screen'), + + Node( + package='mir_restapi', + executable='mir_restapi_client', + output='screen'), + + ]) diff --git a/mir_restapi/mir_restapi/mir_restapi_client.py b/mir_restapi/mir_restapi/mir_restapi_client.py index 09659e34..44811830 100644 --- a/mir_restapi/mir_restapi/mir_restapi_client.py +++ b/mir_restapi/mir_restapi/mir_restapi_client.py @@ -1,55 +1,28 @@ import rclpy -from rclpy.action import ActionClient from rclpy.node import Node +from std_srvs.srv import Trigger -from action_tutorials_interfaces.action import Fibonacci -class FibonacciActionClient(Node): +class MirRestAPIClient(Node): def __init__(self): - super().__init__('fibonacci_action_client') - self._action_client = ActionClient(self, Fibonacci, 'fibonacci') + super().__init__('mir_restapi_client') + self.create_api_clients() - def send_goal(self, order): - goal_msg = Fibonacci.Goal() - goal_msg.order = order - - self._action_client.wait_for_server() - - self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback) - - self._send_goal_future.add_done_callback(self.goal_response_callback) - - def goal_response_callback(self, future): - goal_handle = future.result() - if not goal_handle.accepted: - self.get_logger().info('Goal rejected :(') - return - - self.get_logger().info('Goal accepted :)') - - self._get_result_future = goal_handle.get_result_async() - self._get_result_future.add_done_callback(self.get_result_callback) - - def get_result_callback(self, future): - result = future.result().result - self.get_logger().info('Result: {0}'.format(result.sequence)) - rclpy.shutdown() - - def feedback_callback(self, feedback_msg): - feedback = feedback_msg.feedback - self.get_logger().info('Received feedback: {0}'.format(feedback.partial_sequence)) + def create_api_clients(self): + self.restAPI_setTime = self.create_client( + Trigger, + 'mir100_setTime') def main(args=None): rclpy.init(args=args) - action_client = FibonacciActionClient() - - action_client.send_goal(10) + rest_api_client = MirRestAPIClient() - rclpy.spin(action_client) + rclpy.spin(rest_api_client) + rclpy.shutdown() if __name__ == '__main__': diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index 1f662079..b4c94024 100644 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -2,29 +2,27 @@ import sys import rclpy -from rclpy.action import ActionServer from rclpy.node import Node import mir_restapi.mir_restapi_lib -import mir_restapi.action +from std_srvs.srv import Trigger -class MirRestAPIActionServer(Node): +class MirRestAPIServer(Node): def __init__(self, hostname, auth): - super().__init__('mir_restapi') + super().__init__('mir_restapi_server') self.api_handle = mir_restapi.mir_restapi_lib.MirRestAPI( self.get_logger(), hostname, auth) - # === - # Action server definitions - # === - self.mir100_setTime_action_server = ActionServer( - self, - mir_restapi.action.mir100_setTime, + self.create_api_services() + + def create_api_services(self): + self.restAPI_setTime = self.create_service( + Trigger, 'mir100_setTime', - self.mir100_setTime) + self.api_setTime_callback) def connectRESTapi(self): self.get_logger().info('REST API: Waiting for connection') @@ -38,35 +36,19 @@ def connectRESTapi(self): i += 1 time.sleep(1) - - # === - # Action server callbacks - # === - - def mir100_setTime(self, goal_handle): - self.get_logger().info('Executing goal...') - # Feedback - feedback_msg = mir_restapi.action.mir100_setTime.Feedback() - feedback_msg.settingTime = "true" - self.get_logger().info('Feedback: {}'.format(feedback_msg.settingTime)) - goal_handle.publish_feedback(feedback_msg) - + def api_setTime_callback(self, request, response): + self.get_logger().info('Attempting to setTime through REST API...') self.connectRESTapi() - # options: - # goal_handle.request.options - # Request if self.api_handle.isConnected(printSuccess=False): self.api_handle.setDateTime() # produces an unavoidable connection timeout self.api_handle.close() - - goal_handle.succeed() - - # Result - result = mir_restapi.action.mir100_setTime.Result() - result.setTime = "true" - return result + response.success = True + response.message = "Set Time succesfully" + else: + response.success = False + response.message = "ERROR: Setting Time failed" def main(args=None): @@ -74,9 +56,10 @@ def main(args=None): hostname = "192.168.12.20" auth = "" - mir_restapi_server = MirRestAPIActionServer(hostname, auth) + mir_restapi_server = MirRestAPIServer(hostname, auth) rclpy.spin(mir_restapi_server) + rclpy.shutdown() if __name__ == '__main__': From f93c28bf7a7561b4b969b8e267a0a120c437129b Mon Sep 17 00:00:00 2001 From: niemsoen Date: Thu, 9 Dec 2021 16:04:14 +0100 Subject: [PATCH 21/75] fix for build error? --- mir_restapi/launch/mir_restapi_launch.py | 7 ------- mir_restapi/setup.py | 7 +++++-- 2 files changed, 5 insertions(+), 9 deletions(-) diff --git a/mir_restapi/launch/mir_restapi_launch.py b/mir_restapi/launch/mir_restapi_launch.py index d3bb5518..0fa07259 100644 --- a/mir_restapi/launch/mir_restapi_launch.py +++ b/mir_restapi/launch/mir_restapi_launch.py @@ -1,11 +1,4 @@ -import os - -from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node diff --git a/mir_restapi/setup.py b/mir_restapi/setup.py index de5f6a00..3f4f1b48 100644 --- a/mir_restapi/setup.py +++ b/mir_restapi/setup.py @@ -1,4 +1,6 @@ from setuptools import setup +import os +from glob import glob package_name = 'mir_restapi' @@ -8,9 +10,10 @@ packages=[package_name], data_files=[ ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), + ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), - ], + (os.path.join('share', package_name, 'launch'), glob('launch/*launch.py')), + ], install_requires=['setuptools'], zip_safe=True, maintainer='niemann', From 701a0db7cf186b98248d7350864c61ff5d7de288 Mon Sep 17 00:00:00 2001 From: niemsoen Date: Thu, 9 Dec 2021 16:12:20 +0100 Subject: [PATCH 22/75] wrong directory name --- mir_restapi/{resources => resource}/mir_restapi | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename mir_restapi/{resources => resource}/mir_restapi (100%) diff --git a/mir_restapi/resources/mir_restapi b/mir_restapi/resource/mir_restapi similarity index 100% rename from mir_restapi/resources/mir_restapi rename to mir_restapi/resource/mir_restapi From dbf24fb4a93b93b34440a6b3ec475e48e4154e65 Mon Sep 17 00:00:00 2001 From: niemsoen Date: Thu, 9 Dec 2021 16:23:15 +0100 Subject: [PATCH 23/75] added logging output --- mir_restapi/mir_restapi/mir_restapi_server.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index b4c94024..a6dbe594 100644 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -12,9 +12,11 @@ class MirRestAPIServer(Node): def __init__(self, hostname, auth): super().__init__('mir_restapi_server') + self.get_logger().info("mir_restapi_server started") self.api_handle = mir_restapi.mir_restapi_lib.MirRestAPI( self.get_logger(), hostname, auth) + self.get_logger().info("created MirRestAPI handle") self.create_api_services() @@ -23,6 +25,7 @@ def create_api_services(self): Trigger, 'mir100_setTime', self.api_setTime_callback) + self.get_logger().info("Listening on 'mir100_setTime' for timeset call!") def connectRESTapi(self): self.get_logger().info('REST API: Waiting for connection') From d275c20cbe7a11eeb76325c68c25e68e0b2f78d5 Mon Sep 17 00:00:00 2001 From: niemsoen Date: Thu, 9 Dec 2021 16:31:06 +0100 Subject: [PATCH 24/75] bugfix --- mir_restapi/mir_restapi/mir_restapi_server.py | 1 + 1 file changed, 1 insertion(+) diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index a6dbe594..ddd61dab 100644 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -52,6 +52,7 @@ def api_setTime_callback(self, request, response): else: response.success = False response.message = "ERROR: Setting Time failed" + return response def main(args=None): From e7242c58ff448b6eb9742c53d0e0254f908f4cc8 Mon Sep 17 00:00:00 2001 From: niemsoen Date: Thu, 9 Dec 2021 16:37:11 +0100 Subject: [PATCH 25/75] better logging --- mir_restapi/mir_restapi/mir_restapi_lib.py | 9 +++++---- mir_restapi/mir_restapi/mir_restapi_server.py | 4 +++- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/mir_restapi/mir_restapi/mir_restapi_lib.py b/mir_restapi/mir_restapi/mir_restapi_lib.py index 5ba67710..d891abad 100644 --- a/mir_restapi/mir_restapi/mir_restapi_lib.py +++ b/mir_restapi/mir_restapi/mir_restapi_lib.py @@ -67,18 +67,19 @@ def close(self): self.http.__del__() self.logger.info("REST API: Connection closed") - def isConnected(self, printSuccess=True): + def isConnected(self, print=True): if not self.http.isValid(): self.logger.warn('REST API: Http-Connection is not valid') return False try: self.http.connection.connect() self.http.connection.close() + if print: + self.logger.info("REST API: Connected!") except Exception as e: - self.logger.warn('REST API: Attempt to connect failed: ' + str(e)) + if print: + self.logger.warn('REST API: Attempt to connect failed: ' + str(e)) return False - if printSuccess: - self.logger.info("REST API: Connected!") return True def getStatus(self): diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index ddd61dab..bec27426 100644 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -44,14 +44,16 @@ def api_setTime_callback(self, request, response): self.connectRESTapi() # Request - if self.api_handle.isConnected(printSuccess=False): + if self.api_handle.isConnected(print=False): self.api_handle.setDateTime() # produces an unavoidable connection timeout self.api_handle.close() response.success = True response.message = "Set Time succesfully" + self.get_logger().info(response.message) else: response.success = False response.message = "ERROR: Setting Time failed" + self.get_logger().error(response.message) return response From 611d48799041538884736d740b2864123a259189 Mon Sep 17 00:00:00 2001 From: niemsoen Date: Thu, 9 Dec 2021 16:55:15 +0100 Subject: [PATCH 26/75] added message to response of service call --- mir_restapi/mir_restapi/mir_restapi_lib.py | 5 +---- mir_restapi/mir_restapi/mir_restapi_server.py | 10 ++++++++-- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/mir_restapi/mir_restapi/mir_restapi_lib.py b/mir_restapi/mir_restapi/mir_restapi_lib.py index d891abad..b3305d86 100644 --- a/mir_restapi/mir_restapi/mir_restapi_lib.py +++ b/mir_restapi/mir_restapi/mir_restapi_lib.py @@ -141,10 +141,7 @@ def setDateTime(self): self.logger.info(response) self.logger.info("REST API: Setting time Mir triggers emergency stop, please unlock.") - self.logger.info('REST API: Waiting to close connection: 10s') - time.sleep(5) # this is needed, so that the connection timeout can be ignored - self.logger.info('REST API: Waiting to close connection: 5s') - time.sleep(5) + return response def getDistanceStatistics(self): response = self.http.get("/statistics/distance") diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index bec27426..8bf0cdd4 100644 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -45,10 +45,16 @@ def api_setTime_callback(self, request, response): # Request if self.api_handle.isConnected(print=False): - self.api_handle.setDateTime() # produces an unavoidable connection timeout + # produces an unavoidable connection timeout + response.message = self.api_handle.setDateTime() + # this is needed, so that the connection timeout can be ignored + self.logger.info('REST API: Waiting to close connection: 10s') + time.sleep(5) + self.get_logger().info('REST API: Waiting to close connection: 5s') + time.sleep(5) self.api_handle.close() + response.success = True - response.message = "Set Time succesfully" self.get_logger().info(response.message) else: response.success = False From 179cf08ca6ae406d2923538fb09e7f9ecb2af08b Mon Sep 17 00:00:00 2001 From: niemsoen Date: Thu, 9 Dec 2021 17:03:58 +0100 Subject: [PATCH 27/75] bugfix --- mir_restapi/mir_restapi/mir_restapi_server.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index 8bf0cdd4..6ef0a893 100644 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -48,7 +48,7 @@ def api_setTime_callback(self, request, response): # produces an unavoidable connection timeout response.message = self.api_handle.setDateTime() # this is needed, so that the connection timeout can be ignored - self.logger.info('REST API: Waiting to close connection: 10s') + self.get_logger().info('REST API: Waiting to close connection: 10s') time.sleep(5) self.get_logger().info('REST API: Waiting to close connection: 5s') time.sleep(5) From fcac36879336989b17b3d644435587c88b1f83dd Mon Sep 17 00:00:00 2001 From: niemsoen Date: Thu, 9 Dec 2021 17:14:20 +0100 Subject: [PATCH 28/75] added auth as parameter --- mir_restapi/mir_restapi/mir_restapi_server.py | 27 +++++++++++++------ 1 file changed, 19 insertions(+), 8 deletions(-) diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index 6ef0a893..a0470909 100644 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -6,19 +6,31 @@ import mir_restapi.mir_restapi_lib from std_srvs.srv import Trigger +from rcl_interfaces.msg import SetParametersResult class MirRestAPIServer(Node): - def __init__(self, hostname, auth): + def __init__(self, hostname): super().__init__('mir_restapi_server') self.get_logger().info("mir_restapi_server started") - - self.api_handle = mir_restapi.mir_restapi_lib.MirRestAPI( - self.get_logger(), hostname, auth) - self.get_logger().info("created MirRestAPI handle") - + + self.hostname = hostname self.create_api_services() + + self.declare_parameter('auth', "") + self.auth = self.get_parameter('auth').get_parameter_value().string_value + self.add_on_set_parameters_callback(self.parameters_callback) + + def parameters_callback(self, params): + for param in params: + if param.name == "auth": + self.get_logger().info("Received auth token") + self.auth = param.value + self.api_handle = mir_restapi.mir_restapi_lib.MirRestAPI( + self.get_logger(), self.hostname, self.auth) + self.get_logger().info("created MirRestAPI handle") + return SetParametersResult(successful=True) def create_api_services(self): self.restAPI_setTime = self.create_service( @@ -67,8 +79,7 @@ def main(args=None): rclpy.init(args=args) hostname = "192.168.12.20" - auth = "" - mir_restapi_server = MirRestAPIServer(hostname, auth) + mir_restapi_server = MirRestAPIServer(hostname) rclpy.spin(mir_restapi_server) rclpy.shutdown() From d3e8750db0f6a9a17fd652a2a0094da2f99465fc Mon Sep 17 00:00:00 2001 From: niemsoen Date: Thu, 9 Dec 2021 17:15:45 +0100 Subject: [PATCH 29/75] catch token not set --- mir_restapi/mir_restapi/mir_restapi_server.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index a0470909..5dd1639e 100644 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -52,6 +52,9 @@ def connectRESTapi(self): time.sleep(1) def api_setTime_callback(self, request, response): + if self.auth == "": + self.get_logger().error('API token not set yet') + return self.get_logger().info('Attempting to setTime through REST API...') self.connectRESTapi() From 533d1f03969f2197e54b7bbd4af76bc09407284e Mon Sep 17 00:00:00 2001 From: niemsoen Date: Thu, 9 Dec 2021 17:17:37 +0100 Subject: [PATCH 30/75] bugfix response --- mir_restapi/mir_restapi/mir_restapi_server.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index 5dd1639e..1dcf37e1 100644 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -53,8 +53,10 @@ def connectRESTapi(self): def api_setTime_callback(self, request, response): if self.auth == "": - self.get_logger().error('API token not set yet') - return + response.success = False + response.message = 'API token not set yet' + self.get_logger().error(response.message) + return response self.get_logger().info('Attempting to setTime through REST API...') self.connectRESTapi() From 197aac188dfc0d85994e1362023905b212a95840 Mon Sep 17 00:00:00 2001 From: niemsoen Date: Thu, 9 Dec 2021 18:34:05 +0100 Subject: [PATCH 31/75] added check for error in setting as response to service call --- mir_restapi/mir_restapi/mir_restapi_lib.py | 20 +++++++++---------- mir_restapi/mir_restapi/mir_restapi_server.py | 12 ++++++----- 2 files changed, 17 insertions(+), 15 deletions(-) diff --git a/mir_restapi/mir_restapi/mir_restapi_lib.py b/mir_restapi/mir_restapi/mir_restapi_lib.py index b3305d86..6aa403e8 100644 --- a/mir_restapi/mir_restapi/mir_restapi_lib.py +++ b/mir_restapi/mir_restapi/mir_restapi_lib.py @@ -121,27 +121,27 @@ def setSetting(self, settingID, settingData): return self.http.put("/setting", json.dumps({settingID: settingData})) def setDateTime(self): - timezone = pytz.timezone("Europe/Berlin") + tz_str = "Europe/Berlin" + timezone = pytz.timezone(tz_str) timeobj = datetime.now(timezone) - self.logger.info("REST API: Set Timezone to Europe/Berlin") + self.logger.info("REST API: Set Timezone to " + tz_str) dT = timeobj.strftime("%Y-%m-%dT%X") response = 'REST API: ' try: - response = str(self.http.put("/status", json.dumps({'datetime': dT}))) + response += str(self.http.put("/status", json.dumps({'datetime': dT}))) except Exception as e: if str(e) == "timed out": # setting datetime over REST API seems not to be intended # that's why there is no response accompanying the PUT request, # therefore a time out occurs, however time has been set correctly - response += "Set datetime" - else: - response = "Error setting datetime" - response = response + " to " + dT - - self.logger.info(response) - self.logger.info("REST API: Setting time Mir triggers emergency stop, please unlock.") + response += "Set datetime to " + dT + " in timezone " + tz_str + self.logger.info("REST API: Setting time Mir triggers emergency stop, please unlock.") + self.logger.info(response) + return response + response += " Error setting datetime" return response + def getDistanceStatistics(self): response = self.http.get("/statistics/distance") diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index 1dcf37e1..fb2f5f8b 100644 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -63,7 +63,7 @@ def api_setTime_callback(self, request, response): # Request if self.api_handle.isConnected(print=False): # produces an unavoidable connection timeout - response.message = self.api_handle.setDateTime() + response.message = self.api_handle.setDateTime() # this is needed, so that the connection timeout can be ignored self.get_logger().info('REST API: Waiting to close connection: 10s') time.sleep(5) @@ -71,12 +71,14 @@ def api_setTime_callback(self, request, response): time.sleep(5) self.api_handle.close() - response.success = True - self.get_logger().info(response.message) + if "Error" in response.message: + response.success = False + else: + response.success = True else: response.success = False - response.message = "ERROR: Setting Time failed" - self.get_logger().error(response.message) + response.message = "ERROR: Couldn't connect to REST API" + self.get_logger().error(response.message) return response From fbee422de5f31c8855e0b21c6ee129b4e237cc03 Mon Sep 17 00:00:00 2001 From: niemsoen Date: Thu, 9 Dec 2021 18:43:41 +0100 Subject: [PATCH 32/75] better log --- mir_restapi/mir_restapi/mir_restapi_server.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index fb2f5f8b..fe4cb17d 100644 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -77,7 +77,7 @@ def api_setTime_callback(self, request, response): response.success = True else: response.success = False - response.message = "ERROR: Couldn't connect to REST API" + response.message = "ERROR: Couldn't set time" self.get_logger().error(response.message) return response From a78fb9fd8a456a1dd1b91d4aee7890834573162f Mon Sep 17 00:00:00 2001 From: niemsoen Date: Mon, 20 Dec 2021 18:35:38 +0100 Subject: [PATCH 33/75] introduced parameters for hostname and auth-token --- mir_restapi/mir_restapi/mir_restapi_lib.py | 4 +- mir_restapi/mir_restapi/mir_restapi_server.py | 44 ++++++++++++------- 2 files changed, 31 insertions(+), 17 deletions(-) diff --git a/mir_restapi/mir_restapi/mir_restapi_lib.py b/mir_restapi/mir_restapi/mir_restapi_lib.py index 6aa403e8..71e20e36 100644 --- a/mir_restapi/mir_restapi/mir_restapi_lib.py +++ b/mir_restapi/mir_restapi/mir_restapi_lib.py @@ -59,8 +59,8 @@ def __init__(self, logger, hostname, auth = ""): self.logger = logger if hostname is not None: address = hostname + ":80" - else: - address="192.168.12.20:80" + # else: + # address="192.168.12.20:80" self.http = HttpConnection(logger, address, auth, "/api/v2.0.0") def close(self): diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index fe4cb17d..278d9654 100644 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -11,22 +11,29 @@ class MirRestAPIServer(Node): - def __init__(self, hostname): + def __init__(self): super().__init__('mir_restapi_server') self.get_logger().info("mir_restapi_server started") - self.hostname = hostname self.create_api_services() - self.declare_parameter('auth', "") - self.auth = self.get_parameter('auth').get_parameter_value().string_value + self.declare_parameter('mir_hostname', "") + self.hostname = self.get_parameter('mir_hostname').get_parameter_value().string_value + + self.declare_parameter('mir_restapi_auth', "") + self.auth = self.get_parameter('mir_restapi_auth').get_parameter_value().string_value + self.add_on_set_parameters_callback(self.parameters_callback) def parameters_callback(self, params): for param in params: - if param.name == "auth": + if param.name == "mir_restapi_auth": self.get_logger().info("Received auth token") self.auth = param.value + if param.name == "mir_restapi_auth": + self.get_logger().info("Set mir hostname") + self.hostname = param.value + if self.hostname != "" and self.auth != "" and self.api_handle == None: self.api_handle = mir_restapi.mir_restapi_lib.MirRestAPI( self.get_logger(), self.hostname, self.auth) self.get_logger().info("created MirRestAPI handle") @@ -40,6 +47,9 @@ def create_api_services(self): self.get_logger().info("Listening on 'mir100_setTime' for timeset call!") def connectRESTapi(self): + if self.api_handle == None: + return -1 + self.get_logger().info('REST API: Waiting for connection') i = 1 while not self.api_handle.isConnected(): @@ -47,18 +57,23 @@ def connectRESTapi(self): sys.exit(0) if i > 5: self.get_logger().error('REST API: Could not connect, giving up') - break + return 0 i += 1 time.sleep(1) + return 1 + + def reponse_api_handle_non_existing(self, response): + response.success = False + response.message = 'API token and/or hostname not set yet' + self.get_logger().error(response.message) + return response def api_setTime_callback(self, request, response): - if self.auth == "": - response.success = False - response.message = 'API token not set yet' - self.get_logger().error(response.message) - return response self.get_logger().info('Attempting to setTime through REST API...') - self.connectRESTapi() + + if self.connectRESTapi() == -1: + response = self.reponse_api_handle_non_existing(response) + return response # Request if self.api_handle.isConnected(print=False): @@ -77,7 +92,7 @@ def api_setTime_callback(self, request, response): response.success = True else: response.success = False - response.message = "ERROR: Couldn't set time" + response.message = "ERROR: Couldn't connect to REST API" self.get_logger().error(response.message) return response @@ -85,8 +100,7 @@ def api_setTime_callback(self, request, response): def main(args=None): rclpy.init(args=args) - hostname = "192.168.12.20" - mir_restapi_server = MirRestAPIServer(hostname) + mir_restapi_server = MirRestAPIServer() rclpy.spin(mir_restapi_server) rclpy.shutdown() From 5f2348f166da646b524a37b9035a9f67ade51575 Mon Sep 17 00:00:00 2001 From: niemsoen Date: Fri, 7 Jan 2022 11:55:47 +0100 Subject: [PATCH 34/75] make sure api handle is setup at launch --- mir_restapi/mir_restapi/mir_restapi_client.py | 22 ++++++++++++++++ mir_restapi/mir_restapi/mir_restapi_server.py | 25 +++++++++++-------- 2 files changed, 37 insertions(+), 10 deletions(-) diff --git a/mir_restapi/mir_restapi/mir_restapi_client.py b/mir_restapi/mir_restapi/mir_restapi_client.py index 44811830..1c375fab 100644 --- a/mir_restapi/mir_restapi/mir_restapi_client.py +++ b/mir_restapi/mir_restapi/mir_restapi_client.py @@ -1,6 +1,7 @@ import rclpy from rclpy.node import Node from std_srvs.srv import Trigger +import time @@ -10,6 +11,27 @@ def __init__(self): super().__init__('mir_restapi_client') self.create_api_clients() + T = 10 + self.get_logger().info("started, calling service in %i s" % T) + time.sleep(T) + + # call service + self.get_logger().info("calling time set service") + req = Trigger.Request() + future = self.restAPI_setTime.call_async(req) + rclpy.spin_until_future_complete(self, future, timeout_sec=60) + if not future.done(): + self.get_logger().error("timeout") + else: + # service done + self.get_logger().info("service executed") + res = future.result() + if res.success: + self.get_logger().info(res.message) + else: + self.get_logger().error(res.message) + + def create_api_clients(self): self.restAPI_setTime = self.create_client( Trigger, diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index 278d9654..6e3a13b9 100644 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -13,17 +13,25 @@ class MirRestAPIServer(Node): def __init__(self): super().__init__('mir_restapi_server') - self.get_logger().info("mir_restapi_server started") - - self.create_api_services() + self.get_logger().info("started") + # parameters: hostname, api_token self.declare_parameter('mir_hostname', "") self.hostname = self.get_parameter('mir_hostname').get_parameter_value().string_value - self.declare_parameter('mir_restapi_auth', "") self.auth = self.get_parameter('mir_restapi_auth').get_parameter_value().string_value - self.add_on_set_parameters_callback(self.parameters_callback) + + self.api_handle = None + self.setup_api_handle() + + self.create_services() + + def setup_api_handle(self): + if self.hostname != "" and self.auth != "" and self.api_handle == None: + self.api_handle = mir_restapi.mir_restapi_lib.MirRestAPI( + self.get_logger(), self.hostname, self.auth) + self.get_logger().info("created MirRestAPI handle") def parameters_callback(self, params): for param in params: @@ -33,13 +41,10 @@ def parameters_callback(self, params): if param.name == "mir_restapi_auth": self.get_logger().info("Set mir hostname") self.hostname = param.value - if self.hostname != "" and self.auth != "" and self.api_handle == None: - self.api_handle = mir_restapi.mir_restapi_lib.MirRestAPI( - self.get_logger(), self.hostname, self.auth) - self.get_logger().info("created MirRestAPI handle") + self.setup_api_handle() return SetParametersResult(successful=True) - def create_api_services(self): + def create_services(self): self.restAPI_setTime = self.create_service( Trigger, 'mir100_setTime', From 90960cef742fdc8d7014a4504f5ff36c4e0484c2 Mon Sep 17 00:00:00 2001 From: niemsoen Date: Fri, 7 Jan 2022 12:36:24 +0100 Subject: [PATCH 35/75] better function names --- mir_restapi/mir_restapi/mir_restapi_server.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index 6e3a13b9..3cb6317e 100644 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -51,7 +51,7 @@ def create_services(self): self.api_setTime_callback) self.get_logger().info("Listening on 'mir100_setTime' for timeset call!") - def connectRESTapi(self): + def test_api_connection(self): if self.api_handle == None: return -1 @@ -67,7 +67,7 @@ def connectRESTapi(self): time.sleep(1) return 1 - def reponse_api_handle_non_existing(self, response): + 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) @@ -76,8 +76,8 @@ def reponse_api_handle_non_existing(self, response): def api_setTime_callback(self, request, response): self.get_logger().info('Attempting to setTime through REST API...') - if self.connectRESTapi() == -1: - response = self.reponse_api_handle_non_existing(response) + if self.test_api_connection() == -1: + response = self.reponse_api_handle_not_exists(response) return response # Request From 05cadf1480bb5991c1dc10da05253880f04e460e Mon Sep 17 00:00:00 2001 From: niemsoen Date: Fri, 7 Jan 2022 18:13:02 +0100 Subject: [PATCH 36/75] file rename --- .../launch/{mir_restapi_launch.py => mir_restapi_test_launch.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename mir_restapi/launch/{mir_restapi_launch.py => mir_restapi_test_launch.py} (100%) diff --git a/mir_restapi/launch/mir_restapi_launch.py b/mir_restapi/launch/mir_restapi_test_launch.py similarity index 100% rename from mir_restapi/launch/mir_restapi_launch.py rename to mir_restapi/launch/mir_restapi_test_launch.py From a629aaa457f77a8fdb71c4d867f7c4fae73bdfd8 Mon Sep 17 00:00:00 2001 From: niemsoen Date: Fri, 7 Jan 2022 18:13:42 +0100 Subject: [PATCH 37/75] launch of mir driver triggers launch of restapi_server --- mir_driver/launch/mir_launch.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/mir_driver/launch/mir_launch.py b/mir_driver/launch/mir_launch.py index ad48a4cc..36709e92 100644 --- a/mir_driver/launch/mir_launch.py +++ b/mir_driver/launch/mir_launch.py @@ -78,6 +78,15 @@ def generate_launch_description(): remappings=[('use_sim_time', LaunchConfiguration('use_sim_time'))], parameters=[{'rviz_enabled': 'false'}], output='screen'), + + Node( + package='mir_restapi', + executable='mir_restapi_server', + parameters=[ + {'mir_restapi_auth': LaunchConfiguration('mir_restapi_auth')}, + {'mir_hostname': LaunchConfiguration('mir_hostname')} + ], + output='screen'), Node( condition=IfCondition(LaunchConfiguration('rviz_enabled')), From ae0130033c38df11d9c0f1e38532a599535c342d Mon Sep 17 00:00:00 2001 From: niemsoen Date: Fri, 7 Jan 2022 18:14:16 +0100 Subject: [PATCH 38/75] mir_bridge creates restapi_client node --- mir_driver/mir_driver/mir_bridge.py | 6 ++++-- mir_restapi/mir_restapi/mir_restapi_client.py | 17 ++++++----------- 2 files changed, 10 insertions(+), 13 deletions(-) diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index 4c932b17..a7142691 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -12,6 +12,7 @@ import mir_driver.rosbridge import mir_restapi.mir_restapi +from mir_restapi.mir_restapi.mir_restapi_client import MirRestAPIClient from rclpy_message_converter import message_converter from geometry_msgs.msg import TwistStamped from nav_msgs.msg import Odometry @@ -384,8 +385,9 @@ def __init__(self): # Connect to Mir REST API: set date time # call ROS action server here if needed - - + restapi_client = MirRestAPIClient() + restapi_client.set_mir_time() + # Connect to ROSbridge self.get_logger().info('Trying to connect to ROSbridge at %s:%i...' % (hostname, port)) self.robot = mir_driver.rosbridge.RosbridgeSetup(hostname, port) diff --git a/mir_restapi/mir_restapi/mir_restapi_client.py b/mir_restapi/mir_restapi/mir_restapi_client.py index 1c375fab..97d699a7 100644 --- a/mir_restapi/mir_restapi/mir_restapi_client.py +++ b/mir_restapi/mir_restapi/mir_restapi_client.py @@ -9,13 +9,14 @@ class MirRestAPIClient(Node): def __init__(self): super().__init__('mir_restapi_client') - self.create_api_clients() + self.create_api_clients() - T = 10 - self.get_logger().info("started, calling service in %i s" % T) - time.sleep(T) + def create_api_clients(self): + self.restAPI_setTime = self.create_client( + Trigger, + 'mir100_setTime') - # call service + def set_mir_time(self): self.get_logger().info("calling time set service") req = Trigger.Request() future = self.restAPI_setTime.call_async(req) @@ -30,12 +31,6 @@ def __init__(self): self.get_logger().info(res.message) else: self.get_logger().error(res.message) - - - def create_api_clients(self): - self.restAPI_setTime = self.create_client( - Trigger, - 'mir100_setTime') def main(args=None): From 50c44632ce68fa44c77d931d6122d558ed325912 Mon Sep 17 00:00:00 2001 From: niemsoen Date: Tue, 11 Jan 2022 12:19:07 +0100 Subject: [PATCH 39/75] import error in mir_bridge for MirRestAPIClient --- mir_driver/mir_driver/mir_bridge.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index a7142691..912600ee 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -11,8 +11,7 @@ from collections.abc import Iterable import mir_driver.rosbridge -import mir_restapi.mir_restapi -from mir_restapi.mir_restapi.mir_restapi_client import MirRestAPIClient +#from mir_restapi.mir_restapi.mir_restapi_client import MirRestAPIClient from rclpy_message_converter import message_converter from geometry_msgs.msg import TwistStamped from nav_msgs.msg import Odometry @@ -385,8 +384,8 @@ def __init__(self): # Connect to Mir REST API: set date time # call ROS action server here if needed - restapi_client = MirRestAPIClient() - restapi_client.set_mir_time() + # restapi_client = MirRestAPIClient() + # restapi_client.set_mir_time() # Connect to ROSbridge self.get_logger().info('Trying to connect to ROSbridge at %s:%i...' % (hostname, port)) From 770c2ee845a6c9b539397d0b07ac21963d4348bc Mon Sep 17 00:00:00 2001 From: niemsoen Date: Tue, 11 Jan 2022 12:37:16 +0100 Subject: [PATCH 40/75] include test client for rest api --- .../mir_restapi/mir_restapi_client_test.py | 48 +++++++++++++++++++ mir_restapi/setup.py | 3 +- 2 files changed, 50 insertions(+), 1 deletion(-) create mode 100644 mir_restapi/mir_restapi/mir_restapi_client_test.py diff --git a/mir_restapi/mir_restapi/mir_restapi_client_test.py b/mir_restapi/mir_restapi/mir_restapi_client_test.py new file mode 100644 index 00000000..27034ebf --- /dev/null +++ b/mir_restapi/mir_restapi/mir_restapi_client_test.py @@ -0,0 +1,48 @@ +import rclpy +from rclpy.node import Node +from std_srvs.srv import Trigger +import time + + + +class MirRestAPIClient(Node): + + def __init__(self): + super().__init__('mir_restapi_client') + self.create_api_clients() + + self.set_mir_time() + + def create_api_clients(self): + self.restAPI_setTime = self.create_client( + Trigger, + 'mir100_setTime') + + def set_mir_time(self): + self.get_logger().info("calling time set service") + req = Trigger.Request() + future = self.restAPI_setTime.call_async(req) + rclpy.spin_until_future_complete(self, future, timeout_sec=60) + if not future.done(): + self.get_logger().error("timeout") + else: + # service done + self.get_logger().info("service executed") + res = future.result() + if res.success: + self.get_logger().info(res.message) + else: + self.get_logger().error(res.message) + + +def main(args=None): + rclpy.init(args=args) + + rest_api_client = MirRestAPIClient() + + rclpy.spin(rest_api_client) + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/mir_restapi/setup.py b/mir_restapi/setup.py index 3f4f1b48..afa888fb 100644 --- a/mir_restapi/setup.py +++ b/mir_restapi/setup.py @@ -24,7 +24,8 @@ entry_points={ 'console_scripts': [ 'mir_restapi_server = mir_restapi.mir_restapi_server:main', - 'mir_restapi_client = mir_restapi.mir_restapi_client:main' + 'mir_restapi_client = mir_restapi.mir_restapi_client:main', + 'mir_restapi_client_test = mir_restapi.mir_restapi_client_test:main' ], }, ) From b839166f83135fde4f01ad377b0c65ff6378d431 Mon Sep 17 00:00:00 2001 From: niemsoen Date: Tue, 11 Jan 2022 12:41:34 +0100 Subject: [PATCH 41/75] add sleep for test call --- mir_restapi/mir_restapi/mir_restapi_client_test.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/mir_restapi/mir_restapi/mir_restapi_client_test.py b/mir_restapi/mir_restapi/mir_restapi_client_test.py index 27034ebf..45e53f81 100644 --- a/mir_restapi/mir_restapi/mir_restapi_client_test.py +++ b/mir_restapi/mir_restapi/mir_restapi_client_test.py @@ -10,7 +10,9 @@ class MirRestAPIClient(Node): def __init__(self): super().__init__('mir_restapi_client') self.create_api_clients() - + + self.get_logger().info('Calling timeset service in 10 s') + time.sleep(10) self.set_mir_time() def create_api_clients(self): From c83122b1d61603c3c60f2bc47fd7751ab409cd1d Mon Sep 17 00:00:00 2001 From: niemsoen Date: Tue, 11 Jan 2022 13:16:52 +0100 Subject: [PATCH 42/75] working, launch file not needed --- mir_restapi/launch/mir_restapi_test_launch.py | 19 ------------------- 1 file changed, 19 deletions(-) delete mode 100644 mir_restapi/launch/mir_restapi_test_launch.py diff --git a/mir_restapi/launch/mir_restapi_test_launch.py b/mir_restapi/launch/mir_restapi_test_launch.py deleted file mode 100644 index 0fa07259..00000000 --- a/mir_restapi/launch/mir_restapi_test_launch.py +++ /dev/null @@ -1,19 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description(): - - return LaunchDescription([ - - Node( - package='mir_restapi', - executable='mir_restapi_server', - output='screen'), - - Node( - package='mir_restapi', - executable='mir_restapi_client', - output='screen'), - - ]) From 58f5f94ab21a234bf419939de37dae488ce32817 Mon Sep 17 00:00:00 2001 From: niemsoen Date: Wed, 12 Jan 2022 11:40:15 +0100 Subject: [PATCH 43/75] add client in mir_bridge.py --- mir_driver/mir_driver/mir_bridge.py | 6 +++--- mir_driver/package.xml | 1 + 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index 912600ee..ddee7c44 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -11,7 +11,7 @@ from collections.abc import Iterable import mir_driver.rosbridge -#from mir_restapi.mir_restapi.mir_restapi_client import MirRestAPIClient +from mir_restapi.mir_restapi.mir_restapi_client import MirRestAPIClient from rclpy_message_converter import message_converter from geometry_msgs.msg import TwistStamped from nav_msgs.msg import Odometry @@ -384,8 +384,8 @@ def __init__(self): # Connect to Mir REST API: set date time # call ROS action server here if needed - # restapi_client = MirRestAPIClient() - # restapi_client.set_mir_time() + restapi_client = MirRestAPIClient() + restapi_client.set_mir_time() # Connect to ROSbridge self.get_logger().info('Trying to connect to ROSbridge at %s:%i...' % (hostname, port)) diff --git a/mir_driver/package.xml b/mir_driver/package.xml index 8f66f6b2..6391ec39 100644 --- a/mir_driver/package.xml +++ b/mir_driver/package.xml @@ -25,6 +25,7 @@ sensor_msgs std_msgs tf2_msgs + mir_restapi visualization_msgs twist_stamper From 1a97c709c441954832e5d79cd38ff88e1c7f0191 Mon Sep 17 00:00:00 2001 From: niemsoen Date: Wed, 12 Jan 2022 12:04:48 +0100 Subject: [PATCH 44/75] trying to fix import --- mir_driver/mir_driver/mir_bridge.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index ddee7c44..5314a79d 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -11,7 +11,7 @@ from collections.abc import Iterable import mir_driver.rosbridge -from mir_restapi.mir_restapi.mir_restapi_client import MirRestAPIClient +from mir_restapi_client import MirRestAPIClient from rclpy_message_converter import message_converter from geometry_msgs.msg import TwistStamped from nav_msgs.msg import Odometry From a75c1ff4789545bb775862dcec9b7b872eee8c3a Mon Sep 17 00:00:00 2001 From: niemsoen Date: Wed, 12 Jan 2022 12:11:50 +0100 Subject: [PATCH 45/75] Revert "trying to fix import" This reverts commit 1a97c709c441954832e5d79cd38ff88e1c7f0191. --- mir_driver/mir_driver/mir_bridge.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index 5314a79d..ddee7c44 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -11,7 +11,7 @@ from collections.abc import Iterable import mir_driver.rosbridge -from mir_restapi_client import MirRestAPIClient +from mir_restapi.mir_restapi.mir_restapi_client import MirRestAPIClient from rclpy_message_converter import message_converter from geometry_msgs.msg import TwistStamped from nav_msgs.msg import Odometry From 83bb97390e328d22773edb0e809bf363c701b635 Mon Sep 17 00:00:00 2001 From: Niemann Date: Tue, 18 Jan 2022 19:12:01 +0100 Subject: [PATCH 46/75] fix for import bug --- mir_driver/mir_driver/mir_bridge.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index ddee7c44..2aff7bee 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -11,7 +11,7 @@ from collections.abc import Iterable import mir_driver.rosbridge -from mir_restapi.mir_restapi.mir_restapi_client import MirRestAPIClient +from mir_restapi.mir_restapi_client import MirRestAPIClient from rclpy_message_converter import message_converter from geometry_msgs.msg import TwistStamped from nav_msgs.msg import Odometry From bb3dbda1cc30ec67b7814440df45ec04a31f6fab Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=B6nke=20Niemann?= Date: Wed, 19 Jan 2022 09:15:43 +0100 Subject: [PATCH 47/75] fix message type error --- mir_restapi/mir_restapi/mir_restapi_server.py | 1 + 1 file changed, 1 insertion(+) diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index 3cb6317e..0a6cc8e0 100644 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -95,6 +95,7 @@ def api_setTime_callback(self, request, response): response.success = False else: response.success = True + return response else: response.success = False response.message = "ERROR: Couldn't connect to REST API" From cc6f54faf9a2565c2e799494b2a970a63ae23629 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=B6nke=20Niemann?= Date: Wed, 19 Jan 2022 09:16:06 +0100 Subject: [PATCH 48/75] try to fix connection refusal of rosbridge after restapi --- mir_driver/mir_driver/mir_bridge.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index 2aff7bee..f921ff36 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -386,6 +386,9 @@ def __init__(self): # call ROS action server here if needed restapi_client = MirRestAPIClient() restapi_client.set_mir_time() + + # fix for connection refused? + time.sleep(5) # Connect to ROSbridge self.get_logger().info('Trying to connect to ROSbridge at %s:%i...' % (hostname, port)) From 89841d63504bfc787653290fa01566ee21a33a15 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=B6nke=20Niemann?= Date: Wed, 19 Jan 2022 10:57:09 +0100 Subject: [PATCH 49/75] try to replace sleep after setTime waiting for reboot --- mir_driver/mir_driver/mir_bridge.py | 4 +--- mir_restapi/mir_restapi/mir_restapi_server.py | 12 ++++++++---- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index f921ff36..4583e563 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -386,9 +386,7 @@ def __init__(self): # call ROS action server here if needed restapi_client = MirRestAPIClient() restapi_client.set_mir_time() - - # fix for connection refused? - time.sleep(5) + # Connect to ROSbridge self.get_logger().info('Trying to connect to ROSbridge at %s:%i...' % (hostname, port)) diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index 0a6cc8e0..15c1e3c4 100644 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -85,10 +85,14 @@ def api_setTime_callback(self, request, response): # produces an unavoidable connection timeout response.message = self.api_handle.setDateTime() # this is needed, so that the connection timeout can be ignored - self.get_logger().info('REST API: Waiting to close connection: 10s') - time.sleep(5) - self.get_logger().info('REST API: Waiting to close connection: 5s') - time.sleep(5) + status = "" + while status == "": + self.get_logger().info('REST API: Waiting for Restapi to restart...') + time.sleep(1) + status = self.api_handle.getStatus() + print("status=") + print(status) + self.api_handle.close() if "Error" in response.message: From 6ad1cccc84f0d12b68583b4b164d79f2ba744f18 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=B6nke=20Niemann?= Date: Wed, 19 Jan 2022 11:05:52 +0100 Subject: [PATCH 50/75] catch CannotSendRequest --- mir_restapi/mir_restapi/mir_restapi_server.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index 15c1e3c4..b9f6f1b9 100644 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -1,3 +1,4 @@ +from http.client import CannotSendRequest import time import sys @@ -87,11 +88,13 @@ def api_setTime_callback(self, request, response): # this is needed, so that the connection timeout can be ignored status = "" while status == "": - self.get_logger().info('REST API: Waiting for Restapi to restart...') - time.sleep(1) - status = self.api_handle.getStatus() + try: + status = self.api_handle.getStatus() + except CannotSendRequest: + self.get_logger().info('REST API: Waiting for Restapi to restart...') print("status=") print(status) + time.sleep(1) self.api_handle.close() From ed8edc6e791ec12b9cbd0ae807ebd6c33d2a3dcf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=B6nke=20Niemann?= Date: Wed, 19 Jan 2022 11:10:31 +0100 Subject: [PATCH 51/75] replace print with logger --- mir_restapi/mir_restapi/mir_restapi_server.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index b9f6f1b9..55d1dd93 100644 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -92,8 +92,8 @@ def api_setTime_callback(self, request, response): status = self.api_handle.getStatus() except CannotSendRequest: self.get_logger().info('REST API: Waiting for Restapi to restart...') - print("status=") - print(status) + self.get_logger().info("status=") + self.get_logger().info(status) time.sleep(1) self.api_handle.close() From 38f644c047431e8d0d8212503d33b640f818099f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=B6nke=20Niemann?= Date: Wed, 19 Jan 2022 11:17:52 +0100 Subject: [PATCH 52/75] disable status call --- mir_restapi/mir_restapi/mir_restapi_server.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index 55d1dd93..4f3591eb 100644 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -86,15 +86,15 @@ def api_setTime_callback(self, request, response): # produces an unavoidable connection timeout response.message = self.api_handle.setDateTime() # this is needed, so that the connection timeout can be ignored - status = "" - while status == "": - try: - status = self.api_handle.getStatus() - except CannotSendRequest: - self.get_logger().info('REST API: Waiting for Restapi to restart...') - self.get_logger().info("status=") - self.get_logger().info(status) - time.sleep(1) + # status = "" + # while status == "": + # try: + # status = self.api_handle.getStatus() + # except CannotSendRequest: + # self.get_logger().info('REST API: Waiting for Restapi to restart...') + # self.get_logger().info("status=") + # self.get_logger().info(status) + # time.sleep(1) self.api_handle.close() From caf504d441c3c6e15084515c57de8f3fdfaf01a9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=B6nke=20Niemann?= Date: Wed, 19 Jan 2022 13:45:04 +0100 Subject: [PATCH 53/75] try output of get status call --- mir_restapi/mir_restapi/mir_restapi_server.py | 24 ++++++++++--------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index 4f3591eb..26dfb4c8 100644 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -83,18 +83,20 @@ def api_setTime_callback(self, request, response): # Request if self.api_handle.isConnected(print=False): - # produces an unavoidable connection timeout - response.message = self.api_handle.setDateTime() # this is needed, so that the connection timeout can be ignored - # status = "" - # while status == "": - # try: - # status = self.api_handle.getStatus() - # except CannotSendRequest: - # self.get_logger().info('REST API: Waiting for Restapi to restart...') - # self.get_logger().info("status=") - # self.get_logger().info(status) - # time.sleep(1) + status = "" + while status == "": + try: + status = self.api_handle.getStatus() + except CannotSendRequest: + self.get_logger().info('REST API: Waiting for Restapi to restart...') + self.get_logger().info("status=") + self.get_logger().info(status) + time.sleep(1) + + # produces an unavoidable connection timeout + # response.message = self.api_handle.setDateTime() + self.api_handle.close() From 8eeab98464314307f78bff8a93b1384e8c9422b4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=B6nke=20Niemann?= Date: Wed, 19 Jan 2022 15:46:11 +0100 Subject: [PATCH 54/75] wait for restapi restart working --- mir_restapi/mir_restapi/mir_restapi_server.py | 27 ++++++++++--------- 1 file changed, 14 insertions(+), 13 deletions(-) mode change 100644 => 100755 mir_restapi/mir_restapi/mir_restapi_server.py diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py old mode 100644 new mode 100755 index 26dfb4c8..94fa7988 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -1,6 +1,7 @@ from http.client import CannotSendRequest import time import sys +import json import rclpy from rclpy.node import Node @@ -83,20 +84,20 @@ def api_setTime_callback(self, request, response): # Request if self.api_handle.isConnected(print=False): - # this is needed, so that the connection timeout can be ignored - status = "" - while status == "": - try: - status = self.api_handle.getStatus() - except CannotSendRequest: - self.get_logger().info('REST API: Waiting for Restapi to restart...') - self.get_logger().info("status=") - self.get_logger().info(status) - time.sleep(1) - + # produces an unavoidable connection timeout - # response.message = self.api_handle.setDateTime() - + response.message = self.api_handle.setDateTime() + + # this is needed, so that the connection timeout can be ignored + while True: + if self.api_handle.isConnected(print=False): + status = json.dumps(self.api_handle.getStatus()) + if "service_unavailable" in status: + self.get_logger().info('REST API: Waiting for Restapi to restart...') + else: + self.get_logger().info('REST API available again') + break + time.sleep(1) self.api_handle.close() From 758a8537659941c601c1ae57c5b6466832b40a37 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=B6nke=20Niemann?= Date: Wed, 19 Jan 2022 16:37:16 +0100 Subject: [PATCH 55/75] make waiting for availability more elegant --- mir_restapi/mir_restapi/mir_restapi_lib.py | 25 +++++++++++++++++++ mir_restapi/mir_restapi/mir_restapi_server.py | 13 ---------- 2 files changed, 25 insertions(+), 13 deletions(-) mode change 100644 => 100755 mir_restapi/mir_restapi/mir_restapi_lib.py diff --git a/mir_restapi/mir_restapi/mir_restapi_lib.py b/mir_restapi/mir_restapi/mir_restapi_lib.py old mode 100644 new mode 100755 index 71e20e36..551bbcc5 --- a/mir_restapi/mir_restapi/mir_restapi_lib.py +++ b/mir_restapi/mir_restapi/mir_restapi_lib.py @@ -82,6 +82,23 @@ def isConnected(self, print=True): return False return True + def isAvailable(self): + status = json.dumps(self.getStatus()) + if "service_unavailable" in status: + return False + else: + return True + + def waitForAvailable(self): + while True: + if self.isConnected(print=False): + if self.isAvailable(): + self.logger.info('REST API: available') + break + else: + self.logger.info('REST API: unavailable... waiting') + time.sleep(1) + def getStatus(self): response = self.http.get("/status") return json.loads(response.read()) @@ -138,6 +155,10 @@ def setDateTime(self): response += "Set datetime to " + dT + " in timezone " + tz_str self.logger.info("REST API: Setting time Mir triggers emergency stop, please unlock.") self.logger.info(response) + + # this is needed, because a timeset restarts the restAPI + self.waitForAvailable() + return response response += " Error setting datetime" return response @@ -163,6 +184,10 @@ def getMissionGuid(self, mission_name): missions = self.getMissions() return next((mis["guid"] for mis in missions if mis["name"]==mission_name), None) + def getSounds(self): + response = self.http.get("/sounds") + return json.loads(response.read()) + def moveTo(self, position, mission="MoveTo"): mis_guid = self.getMissionGuid(mission) pos_guid = self.getPoseGuid(position) diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index 94fa7988..1cb070c9 100755 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -87,19 +87,6 @@ def api_setTime_callback(self, request, response): # produces an unavoidable connection timeout response.message = self.api_handle.setDateTime() - - # this is needed, so that the connection timeout can be ignored - while True: - if self.api_handle.isConnected(print=False): - status = json.dumps(self.api_handle.getStatus()) - if "service_unavailable" in status: - self.get_logger().info('REST API: Waiting for Restapi to restart...') - else: - self.get_logger().info('REST API available again') - break - time.sleep(1) - - self.api_handle.close() if "Error" in response.message: response.success = False From 40675b65e1d7f3d6921ae33e8eed33b848e809b4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=B6nke=20Niemann?= Date: Wed, 19 Jan 2022 17:02:59 +0100 Subject: [PATCH 56/75] clean-up --- mir_restapi/README.md | 3 +- .../mir_restapi/mir_restapi_client_test.py | 50 ------------------- mir_restapi/setup.py | 3 +- 3 files changed, 3 insertions(+), 53 deletions(-) delete mode 100644 mir_restapi/mir_restapi/mir_restapi_client_test.py diff --git a/mir_restapi/README.md b/mir_restapi/README.md index 20848071..1ff4c7fd 100644 --- a/mir_restapi/README.md +++ b/mir_restapi/README.md @@ -1,2 +1,3 @@ # mir_restapi -ROS action server that implements calls to the Mir REST API +ROS server node and client node, that implements calls to the Mir REST API. Currently implemented: +- set MiR's time diff --git a/mir_restapi/mir_restapi/mir_restapi_client_test.py b/mir_restapi/mir_restapi/mir_restapi_client_test.py deleted file mode 100644 index 45e53f81..00000000 --- a/mir_restapi/mir_restapi/mir_restapi_client_test.py +++ /dev/null @@ -1,50 +0,0 @@ -import rclpy -from rclpy.node import Node -from std_srvs.srv import Trigger -import time - - - -class MirRestAPIClient(Node): - - def __init__(self): - super().__init__('mir_restapi_client') - self.create_api_clients() - - self.get_logger().info('Calling timeset service in 10 s') - time.sleep(10) - self.set_mir_time() - - def create_api_clients(self): - self.restAPI_setTime = self.create_client( - Trigger, - 'mir100_setTime') - - def set_mir_time(self): - self.get_logger().info("calling time set service") - req = Trigger.Request() - future = self.restAPI_setTime.call_async(req) - rclpy.spin_until_future_complete(self, future, timeout_sec=60) - if not future.done(): - self.get_logger().error("timeout") - else: - # service done - self.get_logger().info("service executed") - res = future.result() - if res.success: - self.get_logger().info(res.message) - else: - self.get_logger().error(res.message) - - -def main(args=None): - rclpy.init(args=args) - - rest_api_client = MirRestAPIClient() - - rclpy.spin(rest_api_client) - rclpy.shutdown() - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/mir_restapi/setup.py b/mir_restapi/setup.py index afa888fb..3f4f1b48 100644 --- a/mir_restapi/setup.py +++ b/mir_restapi/setup.py @@ -24,8 +24,7 @@ entry_points={ 'console_scripts': [ 'mir_restapi_server = mir_restapi.mir_restapi_server:main', - 'mir_restapi_client = mir_restapi.mir_restapi_client:main', - 'mir_restapi_client_test = mir_restapi.mir_restapi_client_test:main' + 'mir_restapi_client = mir_restapi.mir_restapi_client:main' ], }, ) From 1d1a3d5483e501a8679d28448e9ac64dd71e9522 Mon Sep 17 00:00:00 2001 From: Niemann Date: Thu, 20 Jan 2022 16:54:52 +0100 Subject: [PATCH 57/75] removed unused imports --- mir_restapi/mir_restapi/mir_restapi_client.py | 2 -- mir_restapi/mir_restapi/mir_restapi_server.py | 2 -- 2 files changed, 4 deletions(-) diff --git a/mir_restapi/mir_restapi/mir_restapi_client.py b/mir_restapi/mir_restapi/mir_restapi_client.py index 97d699a7..14233c2b 100644 --- a/mir_restapi/mir_restapi/mir_restapi_client.py +++ b/mir_restapi/mir_restapi/mir_restapi_client.py @@ -1,8 +1,6 @@ import rclpy from rclpy.node import Node from std_srvs.srv import Trigger -import time - class MirRestAPIClient(Node): diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index 1cb070c9..cb1a6839 100755 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -1,7 +1,5 @@ -from http.client import CannotSendRequest import time import sys -import json import rclpy from rclpy.node import Node From 28a8e76991ee055775ac697b344ea541d91e9438 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=B6nke=20Niemann?= Date: Wed, 26 Jan 2022 09:46:37 +0100 Subject: [PATCH 58/75] add potential fix --- mir_driver/launch/mir_headless_launch.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/mir_driver/launch/mir_headless_launch.py b/mir_driver/launch/mir_headless_launch.py index f43a0b47..db305791 100644 --- a/mir_driver/launch/mir_headless_launch.py +++ b/mir_driver/launch/mir_headless_launch.py @@ -79,6 +79,15 @@ def generate_launch_description(): parameters=[{'rviz_enabled': 'false'}], output='screen'), + Node( + package='mir_restapi', + executable='mir_restapi_server', + parameters=[ + {'mir_restapi_auth': LaunchConfiguration('mir_restapi_auth')}, + {'mir_hostname': LaunchConfiguration('mir_hostname')} + ], + output='screen'), + Node( condition=IfCondition(LaunchConfiguration('rviz_enabled')), package='rviz2', From 1f0f1c2e7f6327baf438c201a39f8208483b996f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=B6nke=20Niemann?= Date: Wed, 26 Jan 2022 13:38:37 +0100 Subject: [PATCH 59/75] Call mir_restapi_server in tendobot_driver --- mir_driver/launch/mir_headless_launch.py | 9 --------- mir_driver/launch/mir_launch.py | 9 --------- 2 files changed, 18 deletions(-) diff --git a/mir_driver/launch/mir_headless_launch.py b/mir_driver/launch/mir_headless_launch.py index db305791..f43a0b47 100644 --- a/mir_driver/launch/mir_headless_launch.py +++ b/mir_driver/launch/mir_headless_launch.py @@ -79,15 +79,6 @@ def generate_launch_description(): parameters=[{'rviz_enabled': 'false'}], output='screen'), - Node( - package='mir_restapi', - executable='mir_restapi_server', - parameters=[ - {'mir_restapi_auth': LaunchConfiguration('mir_restapi_auth')}, - {'mir_hostname': LaunchConfiguration('mir_hostname')} - ], - output='screen'), - Node( condition=IfCondition(LaunchConfiguration('rviz_enabled')), package='rviz2', diff --git a/mir_driver/launch/mir_launch.py b/mir_driver/launch/mir_launch.py index 36709e92..ad48a4cc 100644 --- a/mir_driver/launch/mir_launch.py +++ b/mir_driver/launch/mir_launch.py @@ -78,15 +78,6 @@ def generate_launch_description(): remappings=[('use_sim_time', LaunchConfiguration('use_sim_time'))], parameters=[{'rviz_enabled': 'false'}], output='screen'), - - Node( - package='mir_restapi', - executable='mir_restapi_server', - parameters=[ - {'mir_restapi_auth': LaunchConfiguration('mir_restapi_auth')}, - {'mir_hostname': LaunchConfiguration('mir_hostname')} - ], - output='screen'), Node( condition=IfCondition(LaunchConfiguration('rviz_enabled')), From 2683c2b6578bb555f48fed330f204adf0fee730c Mon Sep 17 00:00:00 2001 From: niemsoen Date: Wed, 26 Jan 2022 16:07:58 +0100 Subject: [PATCH 60/75] Added api-call abstarction layer & tested api call while bridge connected (#5) --- mir_driver/mir_driver/mir_bridge.py | 2 +- mir_restapi/mir_restapi/mir_restapi_client.py | 20 ++++++++--- mir_restapi/mir_restapi/mir_restapi_lib.py | 2 +- mir_restapi/mir_restapi/mir_restapi_server.py | 35 ++++++++++++------- 4 files changed, 40 insertions(+), 19 deletions(-) diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index 4583e563..bd049f61 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -385,7 +385,7 @@ def __init__(self): # Connect to Mir REST API: set date time # call ROS action server here if needed restapi_client = MirRestAPIClient() - restapi_client.set_mir_time() + restapi_client.syncTime() # Connect to ROSbridge diff --git a/mir_restapi/mir_restapi/mir_restapi_client.py b/mir_restapi/mir_restapi/mir_restapi_client.py index 14233c2b..9b4f9e16 100644 --- a/mir_restapi/mir_restapi/mir_restapi_client.py +++ b/mir_restapi/mir_restapi/mir_restapi_client.py @@ -12,12 +12,15 @@ def __init__(self): def create_api_clients(self): self.restAPI_setTime = self.create_client( Trigger, - 'mir100_setTime') - - def set_mir_time(self): - self.get_logger().info("calling time set service") + 'mir_100_syncTime') + + self.restAPI_getStatus = self.create_client( + Trigger, + 'mir_100_getStatus') + + def call_trigger_service(self, client): req = Trigger.Request() - future = self.restAPI_setTime.call_async(req) + future = client.call_async(req) rclpy.spin_until_future_complete(self, future, timeout_sec=60) if not future.done(): self.get_logger().error("timeout") @@ -30,6 +33,13 @@ def set_mir_time(self): else: self.get_logger().error(res.message) + def syncTime(self): + self.call_trigger_service(self.restAPI_setTime) + + def getStatus(self): + self.call_trigger_service(self.restAPI_getStatus) + + def main(args=None): rclpy.init(args=args) diff --git a/mir_restapi/mir_restapi/mir_restapi_lib.py b/mir_restapi/mir_restapi/mir_restapi_lib.py index 551bbcc5..705ce779 100755 --- a/mir_restapi/mir_restapi/mir_restapi_lib.py +++ b/mir_restapi/mir_restapi/mir_restapi_lib.py @@ -137,7 +137,7 @@ def getGroupSettings(self, groupID): def setSetting(self, settingID, settingData): return self.http.put("/setting", json.dumps({settingID: settingData})) - def setDateTime(self): + def syncTime(self): tz_str = "Europe/Berlin" timezone = pytz.timezone(tz_str) timeobj = datetime.now(timezone) diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index cb1a6839..680131db 100755 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -43,13 +43,19 @@ def parameters_callback(self, params): self.hostname = param.value self.setup_api_handle() return SetParametersResult(successful=True) - + def create_services(self): self.restAPI_setTime = self.create_service( Trigger, - 'mir100_setTime', - self.api_setTime_callback) - self.get_logger().info("Listening on 'mir100_setTime' for timeset call!") + 'mir_100_syncTime', + self.set_time_callback) + self.get_logger().info("Listening on 'mir_100_syncTime'") + + self.restAPI_getStatus = self.create_service( + Trigger, + 'mir_100_getStatus', + self.get_status_callback) + self.get_logger().info("Listening on 'mir_100_getStatus'") def test_api_connection(self): if self.api_handle == None: @@ -72,20 +78,15 @@ def reponse_api_handle_not_exists(self, response): response.message = 'API token and/or hostname not set yet' self.get_logger().error(response.message) return response - - def api_setTime_callback(self, request, response): - self.get_logger().info('Attempting to setTime through REST API...') - + + def call_restapi_function(self, service_fct, request,response): if self.test_api_connection() == -1: response = self.reponse_api_handle_not_exists(response) return response - # Request if self.api_handle.isConnected(print=False): - # produces an unavoidable connection timeout - response.message = self.api_handle.setDateTime() - + response.message = str(service_fct()) if "Error" in response.message: response.success = False else: @@ -97,6 +98,16 @@ def api_setTime_callback(self, request, response): self.get_logger().error(response.message) return response + def set_time_callback(self, request, response): + self.get_logger().info('Syncing host time with REST API...') + response = self.call_restapi_function(self.api_handle.syncTime, request, response) + return response + + def get_status_callback(self, request, response): + self.get_logger().info('Getting status from REST API...') + response = self.call_restapi_function(self.api_handle.getStatus, request, response) + return response + def main(args=None): rclpy.init(args=args) From 3083ba7a94176383ce46ea483c455f5492368c86 Mon Sep 17 00:00:00 2001 From: niemsoen Date: Wed, 26 Jan 2022 16:20:03 +0100 Subject: [PATCH 61/75] init (#6) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Sönke Niemann --- mir_restapi/mir_restapi/mir_restapi_lib.py | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/mir_restapi/mir_restapi/mir_restapi_lib.py b/mir_restapi/mir_restapi/mir_restapi_lib.py index 705ce779..4a5c9114 100755 --- a/mir_restapi/mir_restapi/mir_restapi_lib.py +++ b/mir_restapi/mir_restapi/mir_restapi_lib.py @@ -2,7 +2,6 @@ import time import http.client from datetime import datetime -import pytz class HttpConnection(): @@ -138,11 +137,7 @@ def setSetting(self, settingID, settingData): return self.http.put("/setting", json.dumps({settingID: settingData})) def syncTime(self): - tz_str = "Europe/Berlin" - timezone = pytz.timezone(tz_str) - timeobj = datetime.now(timezone) - self.logger.info("REST API: Set Timezone to " + tz_str) - + timeobj = datetime.now() dT = timeobj.strftime("%Y-%m-%dT%X") response = 'REST API: ' try: @@ -152,7 +147,7 @@ def syncTime(self): # setting datetime over REST API seems not to be intended # that's why there is no response accompanying the PUT request, # therefore a time out occurs, however time has been set correctly - response += "Set datetime to " + dT + " in timezone " + tz_str + response += "Set datetime to " + dT self.logger.info("REST API: Setting time Mir triggers emergency stop, please unlock.") self.logger.info(response) From 0a52e678c566259da77023210f020932c6626f67 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=B6nke=20Niemann?= Date: Thu, 3 Feb 2022 09:58:47 +0100 Subject: [PATCH 62/75] readme section: time sync in ros2 /w restapi --- README.md | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 58ff119c..806eadc3 100644 --- a/README.md +++ b/README.md @@ -280,7 +280,7 @@ to get out of sync quickly (about 1 second per day!). This causes TF transforms timing out etc. and can be seen using `tf_monitor` (the "Max Delay" is about 3.3 seconds in the following example, but should be less than 0.1 seconds): -#### **Using ROS1** (optional) +#### **Determine delay using ROS1** (optional) Since MiR is running a roscore (ROS1), the following tf_monitor can be excecuted after sourcing ROS1 (i.e. noetic) first: @@ -303,7 +303,7 @@ Node: unknown_publisher 418.344 Hz, Average Delay: 0.827575 Max Delay: 3.35237 Node: unknown_publisher(static) 465.362 Hz, Average Delay: 0 Max Delay: 0 ``` -#### **Using ROS2** +#### **Determine delay using ROS2** If you don't have a ROS1 distro installed, you'll need to run the `mir_driver` first and execute the following once a connection is established: @@ -314,9 +314,23 @@ ros2 run tf2_ros tf2_monitor #### **Fix time synchronization manually:** -* go to "Service" -> "Configuration" -> "System settings" -> "Time settings" -> "Set device time on robot" +* In the Mir dashboard (mir.com in the Mir-Wifi), go to "Service" -> "Configuration" -> "System settings" -> "Time settings" -> "Set device time on robot" Use **load from device** to sync with the system time! +#### **Fix time synchronization using ROS2:** + +From the package `mir_restapi` a node called `mir_restapi_server` can be run, which can execute a time sync REST API call from the driver's host machine to the Mir's host. +* Launch the node with the API key and mir hostname's IP address + + ros2 run mir_restapi mir_restapi_server --ros-args -p mir_hostname:='MIR_IP_ADDR' -p mir_restapi_auth:='YOUR_API_KEY' +* Call the time sync service from terminal by invoking + + ros2 service call /mir_100_syncTime std_srvs/Trigger + +#### **After time sync** + +Keep in mind, that the time sync causes the mir_bridge to freeze. Therefore online time syncs are not recommended. + ### Start `move_base` on the robot From 8b27a65ae5fc5935ba0211741fa60bf11e400c83 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=B6nke=20Niemann?= Date: Thu, 3 Feb 2022 10:02:23 +0100 Subject: [PATCH 63/75] remove automatic time sync call at driver start --- mir_driver/mir_driver/mir_bridge.py | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index bd049f61..e9501b4b 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -380,14 +380,7 @@ def __init__(self): global tf_prefix tf_prefix = self.declare_parameter('~tf_prefix', '').value.strip('/') - - - # Connect to Mir REST API: set date time - # call ROS action server here if needed - restapi_client = MirRestAPIClient() - restapi_client.syncTime() - - + # Connect to ROSbridge self.get_logger().info('Trying to connect to ROSbridge at %s:%i...' % (hostname, port)) self.robot = mir_driver.rosbridge.RosbridgeSetup(hostname, port) From f20d342690bfcaadbb9bf736d865c4c926f14b61 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=B6nke=20Niemann?= Date: Thu, 3 Feb 2022 10:26:37 +0100 Subject: [PATCH 64/75] improved naming style in mir_restapi --- README.md | 2 +- mir_restapi/mir_restapi/mir_restapi_client.py | 8 +-- mir_restapi/mir_restapi/mir_restapi_lib.py | 69 +++++++++---------- mir_restapi/mir_restapi/mir_restapi_server.py | 18 ++--- 4 files changed, 48 insertions(+), 49 deletions(-) diff --git a/README.md b/README.md index 806eadc3..44ddaa4c 100644 --- a/README.md +++ b/README.md @@ -325,7 +325,7 @@ From the package `mir_restapi` a node called `mir_restapi_server` can be run, wh ros2 run mir_restapi mir_restapi_server --ros-args -p mir_hostname:='MIR_IP_ADDR' -p mir_restapi_auth:='YOUR_API_KEY' * Call the time sync service from terminal by invoking - ros2 service call /mir_100_syncTime std_srvs/Trigger + ros2 service call /mir_100_sync_time std_srvs/Trigger #### **After time sync** diff --git a/mir_restapi/mir_restapi/mir_restapi_client.py b/mir_restapi/mir_restapi/mir_restapi_client.py index 9b4f9e16..baa10d6a 100644 --- a/mir_restapi/mir_restapi/mir_restapi_client.py +++ b/mir_restapi/mir_restapi/mir_restapi_client.py @@ -12,11 +12,11 @@ def __init__(self): def create_api_clients(self): self.restAPI_setTime = self.create_client( Trigger, - 'mir_100_syncTime') + 'mir_100_sync_time') self.restAPI_getStatus = self.create_client( Trigger, - 'mir_100_getStatus') + 'mir_100_get_status') def call_trigger_service(self, client): req = Trigger.Request() @@ -33,10 +33,10 @@ def call_trigger_service(self, client): else: self.get_logger().error(res.message) - def syncTime(self): + def sync_time(self): self.call_trigger_service(self.restAPI_setTime) - def getStatus(self): + def get_status(self): self.call_trigger_service(self.restAPI_getStatus) diff --git a/mir_restapi/mir_restapi/mir_restapi_lib.py b/mir_restapi/mir_restapi/mir_restapi_lib.py index 4a5c9114..76787a8a 100755 --- a/mir_restapi/mir_restapi/mir_restapi_lib.py +++ b/mir_restapi/mir_restapi/mir_restapi_lib.py @@ -19,14 +19,14 @@ def __init__(self, logger, address, auth, api_prefix): self.logger.warn(str(e)) def __del__(self): - if self.isValid(): + if self.is_valid(): self.connection.close() - def isValid(self): + def is_valid(self): return not self.connection is None def get(self, path): - if not self.isValid(): + if not self.is_valid(): self.connection.connect() self.connection.request("GET", self.api_prefix+path, headers = self.http_headers) resp = self.connection.getresponse() @@ -49,7 +49,7 @@ def put(self, path, body): self.logger.warn("POST failed with status {} and reason: {}".format(resp.status, resp.reason)) return json.loads(resp.read()) - def putNoResponse(self, path, body): + def put_no_response(self, path, body): self.connection.request("PUT", self.api_prefix+path,body=body, headers=self.http_headers) class MirRestAPI(): @@ -66,8 +66,8 @@ def close(self): self.http.__del__() self.logger.info("REST API: Connection closed") - def isConnected(self, print=True): - if not self.http.isValid(): + def is_connected(self, print=True): + if not self.http.is_valid(): self.logger.warn('REST API: Http-Connection is not valid') return False try: @@ -81,46 +81,46 @@ def isConnected(self, print=True): return False return True - def isAvailable(self): - status = json.dumps(self.getStatus()) + def is_available(self): + status = json.dumps(self.get_status()) if "service_unavailable" in status: return False else: return True - def waitForAvailable(self): + def wait_for_available(self): while True: - if self.isConnected(print=False): - if self.isAvailable(): + if self.is_connected(print=False): + if self.is_available(): self.logger.info('REST API: available') break else: self.logger.info('REST API: unavailable... waiting') time.sleep(1) - def getStatus(self): + def get_status(self): response = self.http.get("/status") return json.loads(response.read()) - def getStateId(self): - status = self.getStatus() + 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 setStateId(self, stateId): + def set_state_id(self, stateId): return self.http.put("/status", json.dumps({'state_id': stateId})) - def isReady(self): - status = self.getStatus() + def is_ready(self): + status = self.get_status() if status["state_id"] != 3: # 3=Ready, 4=Pause, 11=Manualcontrol self.logger.warn("MIR currently occupied. System state: {}".format(status["state_text"])) return False else: return True - def getAllSettings(self, advanced=False, listGroups=False): + def get_all_settings(self, advanced=False, listGroups=False): if advanced: response = self.http.get("/settings/advanced") elif listGroups: @@ -129,14 +129,14 @@ def getAllSettings(self, advanced=False, listGroups=False): response = self.http.get("/settings") return json.loads(response.read()) - def getGroupSettings(self, groupID): + def get_group_settings(self, groupID): response = self.http.get("/setting_groups/" + groupID + "/settings") return json.loads(response.read()) - def setSetting(self, settingID, settingData): + def set_setting(self, settingID, settingData): return self.http.put("/setting", json.dumps({settingID: settingData})) - def syncTime(self): + def sync_time(self): timeobj = datetime.now() dT = timeobj.strftime("%Y-%m-%dT%X") response = 'REST API: ' @@ -152,44 +152,43 @@ def syncTime(self): self.logger.info(response) # this is needed, because a timeset restarts the restAPI - self.waitForAvailable() + self.wait_for_available() return response response += " Error setting datetime" return response - - def getDistanceStatistics(self): + def get_distance_statistics(self): response = self.http.get("/statistics/distance") return json.loads(response.read()) - def getPositions(self): + def get_positions(self): response = self.http.get("/positions") return json.loads(response.read()) - def getPoseGuid(self, pos_name): - positions = self.getPositions() + def get_pose_guid(self, pos_name): + positions = self.get_positions() return next((pos["guid"] for pos in positions if pos["name"]==pos_name), None) - def getMissions(self): + def get_missions(self): response = self.http.get("/missions") return json.loads(response.read()) - def getMissionGuid(self, mission_name): - missions = self.getMissions() + def get_mission_guid(self, mission_name): + missions = self.get_missions() return next((mis["guid"] for mis in missions if mis["name"]==mission_name), None) - def getSounds(self): + def get_sounds(self): response = self.http.get("/sounds") return json.loads(response.read()) - def moveTo(self, position, mission="MoveTo"): - mis_guid = self.getMissionGuid(mission) - pos_guid = self.getPoseGuid(position) + def move_to(self, position, mission="move_to"): + mis_guid = self.get_mission_guid(mission) + pos_guid = self.get_pose_guid(position) for (var, txt, name) in zip((mis_guid, pos_guid),("Mission", "Position"),(mission, position)): if var is None: - self.logger.warn("No {} named {} available on MIR - Aborting MoveTo".format(txt,name)) + self.logger.warn("No {} named {} available on MIR - Aborting move_to".format(txt,name)) return body = json.dumps({ diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index 680131db..664b9689 100755 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -47,15 +47,15 @@ def parameters_callback(self, params): def create_services(self): self.restAPI_setTime = self.create_service( Trigger, - 'mir_100_syncTime', + 'mir_100_sync_time', self.set_time_callback) - self.get_logger().info("Listening on 'mir_100_syncTime'") + self.get_logger().info("Listening on 'mir_100_sync_time'") - self.restAPI_getStatus = self.create_service( + self.restAPI_get_status = self.create_service( Trigger, - 'mir_100_getStatus', + 'mir_100_get_status', self.get_status_callback) - self.get_logger().info("Listening on 'mir_100_getStatus'") + self.get_logger().info("Listening on 'mir_100_get_status'") def test_api_connection(self): if self.api_handle == None: @@ -63,7 +63,7 @@ def test_api_connection(self): self.get_logger().info('REST API: Waiting for connection') i = 1 - while not self.api_handle.isConnected(): + while not self.api_handle.is_connected(): if not rclpy.ok(): sys.exit(0) if i > 5: @@ -84,7 +84,7 @@ def call_restapi_function(self, service_fct, request,response): response = self.reponse_api_handle_not_exists(response) return response # Request - if self.api_handle.isConnected(print=False): + if self.api_handle.is_connected(print=False): # produces an unavoidable connection timeout response.message = str(service_fct()) if "Error" in response.message: @@ -100,12 +100,12 @@ def call_restapi_function(self, service_fct, request,response): def set_time_callback(self, request, response): self.get_logger().info('Syncing host time with REST API...') - response = self.call_restapi_function(self.api_handle.syncTime, request, response) + response = self.call_restapi_function(self.api_handle.sync_time, request, response) return response def get_status_callback(self, request, response): self.get_logger().info('Getting status from REST API...') - response = self.call_restapi_function(self.api_handle.getStatus, request, response) + response = self.call_restapi_function(self.api_handle.get_status, request, response) return response From a4b7a4c451700064b4c1054ab1fa32e499841651 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=B6nke=20Niemann?= Date: Thu, 10 Feb 2022 15:32:06 +0100 Subject: [PATCH 65/75] catch case of no api token set, warn user --- mir_restapi/mir_restapi/mir_restapi_server.py | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index 664b9689..93de9391 100755 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -23,22 +23,30 @@ def __init__(self): self.add_on_set_parameters_callback(self.parameters_callback) self.api_handle = None + self.setup_api_handle() - - self.create_services() + + if self.api_handle == None: + self.get_logger().warn(""" + Hostname and API token are not set! Run as follows: + + ros2 run mir_restapi mir_restapi_server --ros-args -p mir_hostname:='MIR_IP_ADDR' -p mir_restapi_auth:='YOUR_API_KEY' + """) def setup_api_handle(self): - if self.hostname != "" and self.auth != "" and self.api_handle == None: + if self.hostname != "" and self.auth != "": self.api_handle = mir_restapi.mir_restapi_lib.MirRestAPI( self.get_logger(), self.hostname, self.auth) 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": self.get_logger().info("Received auth token") self.auth = param.value - if param.name == "mir_restapi_auth": + if param.name == "mir_hostname": self.get_logger().info("Set mir hostname") self.hostname = param.value self.setup_api_handle() @@ -79,7 +87,7 @@ def reponse_api_handle_not_exists(self, response): self.get_logger().error(response.message) return response - def call_restapi_function(self, service_fct, request,response): + def call_restapi_function(self, service_fct, request, response): if self.test_api_connection() == -1: response = self.reponse_api_handle_not_exists(response) return response From 8de5868c2955fa5f85337af6c719863890bae765 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=B6nke=20Niemann?= Date: Thu, 17 Feb 2022 15:48:15 +0100 Subject: [PATCH 66/75] added more untested calls --- mir_restapi/mir_restapi/mir_restapi_lib.py | 60 +++++++++++- mir_restapi/mir_restapi/mir_restapi_server.py | 93 +++++++++++++++++-- 2 files changed, 146 insertions(+), 7 deletions(-) diff --git a/mir_restapi/mir_restapi/mir_restapi_lib.py b/mir_restapi/mir_restapi/mir_restapi_lib.py index 76787a8a..acacbc08 100755 --- a/mir_restapi/mir_restapi/mir_restapi_lib.py +++ b/mir_restapi/mir_restapi/mir_restapi_lib.py @@ -211,4 +211,62 @@ def move_to(self, position, mission="move_to"): self.logger.info(data["state"]) time.sleep(2) - self.logger.info("Mission executed successfully") \ No newline at end of file + self.logger.info("Mission executed successfully") + + def add_mission_to_queue(self, mission_name): + mis_guid = self.get_mission_guid(mission_name) + if mis_guid == None: + self.logger.warn("No Mission named '{}' available on MIR - Aborting move_to".format(mission_name)) + return False + + # put in mission queue + body = json.dumps( + { + "mission_id": str(mis_guid), + "message": "Mission scheduled by ROS node mir_restapi_server", + "priority": 0 + } + ) + + data = self.http.post("/mission_queue", body) + try: + self.logger.info("Mission scheduled for execution under id {}".format(data["id"])) + return True + except KeyError: + self.logger.warn("Couldn't schedule mission") + self.logger.warn(str(data)) + return False + + def is_mission_in_queue(self, mission_name): + try: + mis_guid = self.get_mission_guid(mission_name) + response = self.http.get("/mission_queue/{}".format(mis_guid)) + except http.client.ResponseNotReady or http.client.CannotSendRequest: + self.logger.info("Http error: Mission {} is still in queue".format(mission_name)) + self.http.__del__() + return True + + data = json.loads(response.read()) + self.logger.info("data") + + if response.status == 404: + self.logger.info("Mission {} is done".format(mission_name)) + return False + self.logger.info("Mission {} is still in queue".format(mission_name)) + return True + + def is_mission_queue_empty(self): + try: + response = self.http.get("/mission_queue") + data = json.loads(response.read()) + self.logger.info(data) + except http.client.ResponseNotReady or http.client.CannotSendRequest: + self.logger.info("Http error") + self.http.__del__() + return False + + # if response.status == 404: + # self.logger.info("Mission {} is done".format(mission_name)) + # return False + # self.logger.info("Mission {} is still in queue".format(mission_name)) + return True \ No newline at end of file diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index 93de9391..4868e1da 100755 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -56,7 +56,7 @@ def create_services(self): self.restAPI_setTime = self.create_service( Trigger, 'mir_100_sync_time', - self.set_time_callback) + self.sync_time_callback) self.get_logger().info("Listening on 'mir_100_sync_time'") self.restAPI_get_status = self.create_service( @@ -64,6 +64,30 @@ def create_services(self): 'mir_100_get_status', self.get_status_callback) self.get_logger().info("Listening on 'mir_100_get_status'") + + self.restAPI_get_sounds = self.create_service( + Trigger, + 'mir_100_get_sounds', + self.get_sounds_callback) + self.get_logger().info("Listening on 'mir_100_get_sounds'") + + self.restAPI_is_emergency_halt = self.create_service( + Trigger, + 'mir_100_is_emergency_halt', + self.is_emergency_halt_callback) + self.get_logger().info("Listening on 'mir_100_is_emergency_halt'") + + self.restAPI_get_missions = self.create_service( + Trigger, + 'mir_100_get_missions', + self.get_missions_callback) + self.get_logger().info("Listening on 'mir_100_get_missions'") + + self.restAPI_honk = self.create_service( + Trigger, + 'mir_100_honk', + self.honk) + self.get_logger().info("Listening on 'mir_100_honk'") def test_api_connection(self): if self.api_handle == None: @@ -87,14 +111,15 @@ def reponse_api_handle_not_exists(self, response): self.get_logger().error(response.message) return response - def call_restapi_function(self, service_fct, request, 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) return response - # Request if self.api_handle.is_connected(print=False): - # produces an unavoidable connection timeout - response.message = str(service_fct()) + if args==None: + response.message = str(service_fct()) + else: + response.message = str(service_fct(args)) if "Error" in response.message: response.success = False else: @@ -106,7 +131,7 @@ def call_restapi_function(self, service_fct, request, response): self.get_logger().error(response.message) return response - def set_time_callback(self, request, response): + def sync_time_callback(self, request, response): self.get_logger().info('Syncing host time with REST API...') response = self.call_restapi_function(self.api_handle.sync_time, request, response) return response @@ -115,6 +140,62 @@ 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...') + 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) + STATE_ID_EMERGENCY = 10 + if state_id == STATE_ID_EMERGENCY: + response.message = str(True) + else: + response.message = str(False) + 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(self, request, response): + self.get_logger().info('Honking horn over REST API...') + + mission_name = "honk" + + response_miss_queue = self.call_restapi_function(self.api_handle.add_mission_to_queue, request, response, args=mission_name) + if response_miss_queue.message == str(False): + response.message = "Honking failed due to mission queue error" + self.get_logger().error(response.message) + response.success = False + return response + + emerg_response = self.is_emergency_halt_callback(request, response) + if emerg_response.message == str(True): + response.message = "Can't honk, emergency halt" + self.get_logger().error(response.message) + response.success = False + else: + response.message = "Honking" + self.get_logger().info(response.message) + STATE_ID_RUN_MISSION = 3 + STATE_ID_PAUSE = 4 + + self.api_handle.set_state_id(STATE_ID_RUN_MISSION) + + # while self.api_handle.is_mission_queue_empty(): + time.sleep(5) + + self.api_handle.set_state_id(STATE_ID_PAUSE) + response.success = True + return response def main(args=None): From 702be63f7dfd42d80282984f53a9b357574bf68b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=B6nke=20Niemann?= Date: Thu, 17 Feb 2022 17:23:12 +0100 Subject: [PATCH 67/75] added waiting condition for honking --- mir_restapi/mir_restapi/mir_restapi_lib.py | 50 ++++++++----------- mir_restapi/mir_restapi/mir_restapi_server.py | 14 ++++-- 2 files changed, 29 insertions(+), 35 deletions(-) diff --git a/mir_restapi/mir_restapi/mir_restapi_lib.py b/mir_restapi/mir_restapi/mir_restapi_lib.py index acacbc08..15bd15a7 100755 --- a/mir_restapi/mir_restapi/mir_restapi_lib.py +++ b/mir_restapi/mir_restapi/mir_restapi_lib.py @@ -217,7 +217,7 @@ def add_mission_to_queue(self, mission_name): mis_guid = self.get_mission_guid(mission_name) if mis_guid == None: self.logger.warn("No Mission named '{}' available on MIR - Aborting move_to".format(mission_name)) - return False + return False, -1 # put in mission queue body = json.dumps( @@ -231,42 +231,32 @@ def add_mission_to_queue(self, mission_name): data = self.http.post("/mission_queue", body) try: self.logger.info("Mission scheduled for execution under id {}".format(data["id"])) - return True + return True, int(data["id"]) except KeyError: self.logger.warn("Couldn't schedule mission") self.logger.warn(str(data)) - return False - - def is_mission_in_queue(self, mission_name): - try: - mis_guid = self.get_mission_guid(mission_name) - response = self.http.get("/mission_queue/{}".format(mis_guid)) - except http.client.ResponseNotReady or http.client.CannotSendRequest: - self.logger.info("Http error: Mission {} is still in queue".format(mission_name)) - self.http.__del__() - return True - - data = json.loads(response.read()) - self.logger.info("data") - - if response.status == 404: - self.logger.info("Mission {} is done".format(mission_name)) - return False - self.logger.info("Mission {} is still in queue".format(mission_name)) - return True + return False, -1 - def is_mission_queue_empty(self): + def is_mission_done(self, mission_queue_id): try: + # mis_guid = self.get_mission_guid(mission_name) response = self.http.get("/mission_queue") - data = json.loads(response.read()) - self.logger.info(data) + except http.client.ResponseNotReady or http.client.CannotSendRequest: - self.logger.info("Http error") + self.logger.info("Http error: Mission with queue_id {} is still in queue".format(mission_queue_id)) self.http.__del__() return False - # if response.status == 404: - # self.logger.info("Mission {} is done".format(mission_name)) - # return False - # self.logger.info("Mission {} is still in queue".format(mission_name)) - return True \ No newline at end of file + # self.logger.info("Mission with queue_id {} is in queue".format(mission_queue_id)) + # self.logger.info("Response status {}".format(response.status)) + data = json.loads(response.read()) + + for d in data: + if d["id"]==mission_queue_id: + if d["state"] == 'Done': + self.logger.info("Mission {} is done".format(mission_queue_id)) + return True + + self.logger.info("Mission with queue_id {} is still in queue".format(mission_queue_id)) + return False + diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index 4868e1da..de635205 100755 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -152,12 +152,14 @@ def is_emergency_halt_callback(self, request, response): if response.success: state_id = int(response.message) - self.get_logger().info("Returned state_id as %i" % state_id) + #self.get_logger().info("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") else: response.message = str(False) + # self.get_logger().info("no emergency halt") return response def get_missions_callback(self, request, response): @@ -170,12 +172,13 @@ def honk(self, request, response): mission_name = "honk" - response_miss_queue = self.call_restapi_function(self.api_handle.add_mission_to_queue, request, response, args=mission_name) - if response_miss_queue.message == str(False): + queue_success, mission_queue_id = self.api_handle.add_mission_to_queue(mission_name) + if not queue_success: response.message = "Honking failed due to mission queue error" self.get_logger().error(response.message) response.success = False return response + self.get_logger().info("Put honk mission into queue with mission_queue_id={}".format(mission_queue_id)) emerg_response = self.is_emergency_halt_callback(request, response) if emerg_response.message == str(True): @@ -190,10 +193,11 @@ def honk(self, request, response): self.api_handle.set_state_id(STATE_ID_RUN_MISSION) - # while self.api_handle.is_mission_queue_empty(): - time.sleep(5) + while not self.api_handle.is_mission_done(mission_queue_id): + time.sleep(1) self.api_handle.set_state_id(STATE_ID_PAUSE) + self.api_handle.http.__del__() response.success = True return response From e04224f3eab8cefe0500b19f741d25f5009a3c85 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=B6nke=20Niemann?= Date: Thu, 17 Feb 2022 17:53:59 +0100 Subject: [PATCH 68/75] failed try to find system time in restapi --- mir_restapi/mir_restapi/mir_restapi_lib.py | 4 ++ mir_restapi/mir_restapi/mir_restapi_server.py | 38 +++++++++++++++---- 2 files changed, 34 insertions(+), 8 deletions(-) diff --git a/mir_restapi/mir_restapi/mir_restapi_lib.py b/mir_restapi/mir_restapi/mir_restapi_lib.py index 15bd15a7..cf753830 100755 --- a/mir_restapi/mir_restapi/mir_restapi_lib.py +++ b/mir_restapi/mir_restapi/mir_restapi_lib.py @@ -260,3 +260,7 @@ def is_mission_done(self, mission_queue_id): self.logger.info("Mission with queue_id {} is still in queue".format(mission_queue_id)) return False + def get_system_info(self): + response = self.http.get("/system/info") + return json.loads(response.read()) + diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index de635205..80c1af53 100755 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -53,41 +53,53 @@ def parameters_callback(self, params): return SetParametersResult(successful=True) def create_services(self): - self.restAPI_setTime = self.create_service( + self.create_service( Trigger, 'mir_100_sync_time', self.sync_time_callback) self.get_logger().info("Listening on 'mir_100_sync_time'") - self.restAPI_get_status = self.create_service( + self.create_service( Trigger, 'mir_100_get_status', self.get_status_callback) self.get_logger().info("Listening on 'mir_100_get_status'") - self.restAPI_get_sounds = self.create_service( + self.create_service( Trigger, 'mir_100_get_sounds', self.get_sounds_callback) self.get_logger().info("Listening on 'mir_100_get_sounds'") - self.restAPI_is_emergency_halt = self.create_service( + self.create_service( Trigger, 'mir_100_is_emergency_halt', self.is_emergency_halt_callback) self.get_logger().info("Listening on 'mir_100_is_emergency_halt'") - self.restAPI_get_missions = self.create_service( + self.create_service( Trigger, 'mir_100_get_missions', self.get_missions_callback) self.get_logger().info("Listening on 'mir_100_get_missions'") - self.restAPI_honk = self.create_service( + self.create_service( Trigger, 'mir_100_honk', - self.honk) + self.honk_callback) self.get_logger().info("Listening on 'mir_100_honk'") + + self.create_service( + Trigger, + 'mir_100_get_system_info', + self.get_system_info_callback) + self.get_logger().info("Listening on 'mir_100_get_system_info'") + + self.create_service( + Trigger, + '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 == None: @@ -167,7 +179,7 @@ def get_missions_callback(self, request, response): response = self.call_restapi_function(self.api_handle.get_missions, request, response) return response - def honk(self, request, response): + def honk_callback(self, request, response): self.get_logger().info('Honking horn over REST API...') mission_name = "honk" @@ -200,6 +212,16 @@ def honk(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) + return response + + def get_settings_callback(self, request, response): + self.get_logger().info('Getting settings from REST API...') + response = self.call_restapi_function(self.api_handle.get_all_settings, request, response) + return response def main(args=None): From 367e585d9c583a2d5e49956e48a6809078e00ad2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=B6nke=20Niemann?= Date: Thu, 17 Feb 2022 18:00:49 +0100 Subject: [PATCH 69/75] added client for time sync --- mir_driver/mir_driver/mir_bridge.py | 1 - ...{mir_restapi_client.py => mir_restapi_sync_time.py} | 10 ++++++---- mir_restapi/setup.py | 2 +- 3 files changed, 7 insertions(+), 6 deletions(-) rename mir_restapi/mir_restapi/{mir_restapi_client.py => mir_restapi_sync_time.py} (88%) diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index e9501b4b..4079a29f 100755 --- a/mir_driver/mir_driver/mir_bridge.py +++ b/mir_driver/mir_driver/mir_bridge.py @@ -11,7 +11,6 @@ from collections.abc import Iterable import mir_driver.rosbridge -from mir_restapi.mir_restapi_client import MirRestAPIClient from rclpy_message_converter import message_converter from geometry_msgs.msg import TwistStamped from nav_msgs.msg import Odometry diff --git a/mir_restapi/mir_restapi/mir_restapi_client.py b/mir_restapi/mir_restapi/mir_restapi_sync_time.py similarity index 88% rename from mir_restapi/mir_restapi/mir_restapi_client.py rename to mir_restapi/mir_restapi/mir_restapi_sync_time.py index baa10d6a..f244f07d 100644 --- a/mir_restapi/mir_restapi/mir_restapi_client.py +++ b/mir_restapi/mir_restapi/mir_restapi_sync_time.py @@ -6,8 +6,12 @@ class MirRestAPIClient(Node): def __init__(self): - super().__init__('mir_restapi_client') - self.create_api_clients() + super().__init__('mir_restapi_client_sync_time') + self.create_api_clients() + + self.sync_time() + rclpy.shutdown() + exit() def create_api_clients(self): self.restAPI_setTime = self.create_client( @@ -46,8 +50,6 @@ def main(args=None): rest_api_client = MirRestAPIClient() - rclpy.spin(rest_api_client) - rclpy.shutdown() if __name__ == '__main__': diff --git a/mir_restapi/setup.py b/mir_restapi/setup.py index 3f4f1b48..a6646733 100644 --- a/mir_restapi/setup.py +++ b/mir_restapi/setup.py @@ -24,7 +24,7 @@ entry_points={ 'console_scripts': [ 'mir_restapi_server = mir_restapi.mir_restapi_server:main', - 'mir_restapi_client = mir_restapi.mir_restapi_client:main' + 'mir_restapi_sync_time = mir_restapi.mir_restapi_sync_time:main' ], }, ) From 37fca2916af815eb560c0138ae6d60fae7f65e24 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=B6nke=20Niemann?= Date: Thu, 17 Feb 2022 18:03:28 +0100 Subject: [PATCH 70/75] wait for service available --- mir_restapi/mir_restapi/mir_restapi_sync_time.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/mir_restapi/mir_restapi/mir_restapi_sync_time.py b/mir_restapi/mir_restapi/mir_restapi_sync_time.py index f244f07d..87e9cd75 100644 --- a/mir_restapi/mir_restapi/mir_restapi_sync_time.py +++ b/mir_restapi/mir_restapi/mir_restapi_sync_time.py @@ -9,6 +9,8 @@ def __init__(self): super().__init__('mir_restapi_client_sync_time') self.create_api_clients() + self.restAPI_setTime.wait_for_service() + self.sync_time() rclpy.shutdown() exit() From 0088f262d55ac6fcceb9b843556568f8bfb41f21 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=B6nke=20Niemann?= Date: Wed, 2 Mar 2022 13:47:39 +0100 Subject: [PATCH 71/75] added requested changes --- mir_driver/package.xml | 1 - mir_restapi/mir_restapi/mir_restapi_lib.py | 2 +- mir_restapi/package.xml | 5 +++++ 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/mir_driver/package.xml b/mir_driver/package.xml index 6391ec39..8f66f6b2 100644 --- a/mir_driver/package.xml +++ b/mir_driver/package.xml @@ -25,7 +25,6 @@ sensor_msgs std_msgs tf2_msgs - mir_restapi visualization_msgs twist_stamper diff --git a/mir_restapi/mir_restapi/mir_restapi_lib.py b/mir_restapi/mir_restapi/mir_restapi_lib.py index cf753830..290e7073 100755 --- a/mir_restapi/mir_restapi/mir_restapi_lib.py +++ b/mir_restapi/mir_restapi/mir_restapi_lib.py @@ -148,7 +148,7 @@ def sync_time(self): # that's why there is no response accompanying the PUT request, # therefore a time out occurs, however time has been set correctly response += "Set datetime to " + dT - self.logger.info("REST API: Setting time Mir triggers emergency stop, please unlock.") + self.logger.warn("REST API: Setting time Mir triggers emergency stop, please unlock.") self.logger.info(response) # this is needed, because a timeset restarts the restAPI diff --git a/mir_restapi/package.xml b/mir_restapi/package.xml index 49c9f97d..24e9b8b7 100644 --- a/mir_restapi/package.xml +++ b/mir_restapi/package.xml @@ -7,6 +7,11 @@ niemann TODO: License declaration + rclpy + std_msgs + std_srvs + rcl_interfaces + ament_copyright ament_flake8 ament_pep257 From 09c0394b363e29e4049663d20641a096f8f88e29 Mon Sep 17 00:00:00 2001 From: niemsoen Date: Tue, 29 Mar 2022 17:45:38 +0200 Subject: [PATCH 72/75] Add emergency publisher (#7) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * add publisher for emergency halt * fix cyclic emergency halt checking interrupts rosbridge * improved terminal output Co-authored-by: Sönke Niemann --- mir_driver/mir_driver/mir_bridge.py | 36 ++++++++++++++++++- mir_restapi/mir_restapi/mir_restapi_lib.py | 2 +- mir_restapi/mir_restapi/mir_restapi_server.py | 8 ++--- 3 files changed, 40 insertions(+), 6 deletions(-) diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py index ac254e04..e2eb7440 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 from rclpy.time import Time from rclpy.clock import ClockType @@ -17,6 +17,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 = '' @@ -419,8 +420,41 @@ def __init__(self): 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/mir_restapi_lib.py b/mir_restapi/mir_restapi/mir_restapi_lib.py index 290e7073..a7acc0c9 100755 --- a/mir_restapi/mir_restapi/mir_restapi_lib.py +++ b/mir_restapi/mir_restapi/mir_restapi_lib.py @@ -74,7 +74,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)) diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index 80c1af53..6562db2c 100755 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -105,7 +105,7 @@ def test_api_connection(self): 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(): @@ -159,7 +159,7 @@ def get_sounds_callback(self, 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: @@ -168,10 +168,10 @@ def is_emergency_halt_callback(self, request, response): 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): From 726447e3eb28a0a9b495cdfe6ec9dc801a7d8177 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=B6nke=20Niemann?= Date: Tue, 29 Mar 2022 18:35:04 +0200 Subject: [PATCH 73/75] merged upstream changes --- mir_restapi/mir_restapi/mir_restapi_lib.py | 100 +++++++++++---------- 1 file changed, 52 insertions(+), 48 deletions(-) diff --git a/mir_restapi/mir_restapi/mir_restapi_lib.py b/mir_restapi/mir_restapi/mir_restapi_lib.py index 290e7073..e6f506a9 100755 --- a/mir_restapi/mir_restapi/mir_restapi_lib.py +++ b/mir_restapi/mir_restapi/mir_restapi_lib.py @@ -9,9 +9,9 @@ def __init__(self, logger, address, auth, api_prefix): self.logger = logger self.api_prefix = api_prefix self.http_headers = { - "Accept-Language":"en-EN", - "Authorization":auth, - "Content-Type":"application/json"} + "Accept-Language": "en-EN", + "Authorization": auth, + "Content-Type": "application/json"} try: self.connection = http.client.HTTPConnection(host=address, timeout=5) except Exception as e: @@ -23,38 +23,42 @@ def __del__(self): self.connection.close() def is_valid(self): - return not self.connection is None + return self.connection is not None def get(self, path): if not self.is_valid(): self.connection.connect() - self.connection.request("GET", self.api_prefix+path, headers = self.http_headers) - resp = self.connection.getresponse() - if resp.status <200 or resp.status>=300: - self.logger.warn("GET failed with status {} and reason: {}".format(resp.status, resp.reason)) + self.connection.request("GET", self.api_prefix+path, headers=self.http_headers) + resp = self.connection.getresponse() + if resp.status < 200 or resp.status >= 300: + self.logger.warn("GET failed with status {} and reason: {}".format(resp.status, + resp.reason)) return resp def post(self, path, body): - self.connection.request("POST", self.api_prefix+path,body=body, headers=self.http_headers) + self.connection.request("POST", self.api_prefix+path, body=body, headers=self.http_headers) resp = self.connection.getresponse() - if resp.status <200 or resp.status>=300: - self.logger.warn("POST failed with status {} and reason: {}".format(resp.status, resp.reason)) + if resp.status < 200 or resp.status >= 300: + self.logger.warn("POST failed with status {} and reason: {}".format( + resp.status, resp.reason)) return json.loads(resp.read()) def put(self, path, body): - self.connection.request("PUT", self.api_prefix+path,body=body, headers=self.http_headers) + self.connection.request("PUT", self.api_prefix+path, body=body, headers=self.http_headers) resp = self.connection.getresponse() - #self.logger.info(resp.read()) - if resp.status <200 or resp.status>=300: - self.logger.warn("POST failed with status {} and reason: {}".format(resp.status, resp.reason)) + # self.logger.info(resp.read()) + if resp.status < 200 or resp.status >= 300: + self.logger.warn("POST failed with status {} and reason: {}".format( + resp.status, resp.reason)) return json.loads(resp.read()) - + def put_no_response(self, path, body): - self.connection.request("PUT", self.api_prefix+path,body=body, headers=self.http_headers) + self.connection.request("PUT", self.api_prefix+path, body=body, headers=self.http_headers) + class MirRestAPI(): - def __init__(self, logger, hostname, auth = ""): + def __init__(self, logger, hostname, auth=""): self.logger = logger if hostname is not None: address = hostname + ":80" @@ -87,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): @@ -101,25 +105,26 @@ def wait_for_available(self): def get_status(self): response = self.http.get("/status") return json.loads(response.read()) - + 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): return self.http.put("/status", json.dumps({'state_id': stateId})) - + def is_ready(self): status = self.get_status() - if status["state_id"] != 3: # 3=Ready, 4=Pause, 11=Manualcontrol - self.logger.warn("MIR currently occupied. System state: {}".format(status["state_text"])) + if status["state_id"] != 3: # 3=Ready, 4=Pause, 11=Manualcontrol + self.logger.warn("MIR currently occupied. System state: {}".format( + status["state_text"])) return False else: return True - + def get_all_settings(self, advanced=False, listGroups=False): if advanced: response = self.http.get("/settings/advanced") @@ -128,14 +133,14 @@ def get_all_settings(self, advanced=False, listGroups=False): else: response = self.http.get("/settings") return json.loads(response.read()) - + def get_group_settings(self, groupID): response = self.http.get("/setting_groups/" + groupID + "/settings") return json.loads(response.read()) - + def set_setting(self, settingID, settingData): return self.http.put("/setting", json.dumps({settingID: settingData})) - + def sync_time(self): timeobj = datetime.now() dT = timeobj.strftime("%Y-%m-%dT%X") @@ -186,9 +191,11 @@ def move_to(self, position, mission="move_to"): mis_guid = self.get_mission_guid(mission) pos_guid = self.get_pose_guid(position) - for (var, txt, name) in zip((mis_guid, pos_guid),("Mission", "Position"),(mission, position)): + for (var, txt, name) in zip((mis_guid, pos_guid), ("Mission", "Position"), + (mission, position)): if var is None: - self.logger.warn("No {} named {} available on MIR - Aborting move_to".format(txt,name)) + self.logger.warn( + "No {} named {} available on MIR - Aborting move_to".format(txt, name)) return body = json.dumps({ @@ -212,21 +219,18 @@ def move_to(self, position, mission="move_to"): time.sleep(2) self.logger.info("Mission executed successfully") - + def add_mission_to_queue(self, mission_name): mis_guid = self.get_mission_guid(mission_name) - if mis_guid == None: - self.logger.warn("No Mission named '{}' available on MIR - Aborting move_to".format(mission_name)) + if mis_guid is None: + self.logger.warn( + "No Mission named '{}' available on MIR - Aborting move_to".format(mission_name)) return False, -1 # put in mission queue - body = json.dumps( - { - "mission_id": str(mis_guid), - "message": "Mission scheduled by ROS node mir_restapi_server", - "priority": 0 - } - ) + body = json.dumps({"mission_id": str(mis_guid), + "message": "Mission scheduled by ROS node mir_restapi_server", + "priority": 0}) data = self.http.post("/mission_queue", body) try: @@ -236,31 +240,31 @@ 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) response = self.http.get("/mission_queue") - + except http.client.ResponseNotReady or http.client.CannotSendRequest: - self.logger.info("Http error: Mission with queue_id {} is still in queue".format(mission_queue_id)) + self.logger.info( + "Http error: Mission with queue_id {} is still in queue".format(mission_queue_id)) self.http.__del__() return False # self.logger.info("Mission with queue_id {} is in queue".format(mission_queue_id)) # self.logger.info("Response status {}".format(response.status)) data = json.loads(response.read()) - + for d in data: - if d["id"]==mission_queue_id: + if d["id"] == mission_queue_id: if d["state"] == 'Done': self.logger.info("Mission {} is done".format(mission_queue_id)) return True - + self.logger.info("Mission with queue_id {} is still in queue".format(mission_queue_id)) return False - + def get_system_info(self): response = self.http.get("/system/info") return json.loads(response.read()) - From 72f7f8bdcc713d85aa18cd0176873ad52410ef64 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=B6nke=20Niemann?= Date: Tue, 29 Mar 2022 18:47:09 +0200 Subject: [PATCH 74/75] changed logger level to debug for emergency message --- mir_restapi/mir_restapi/mir_restapi_lib.py | 2 +- mir_restapi/mir_restapi/mir_restapi_server.py | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/mir_restapi/mir_restapi/mir_restapi_lib.py b/mir_restapi/mir_restapi/mir_restapi_lib.py index e6f506a9..da4afb18 100755 --- a/mir_restapi/mir_restapi/mir_restapi_lib.py +++ b/mir_restapi/mir_restapi/mir_restapi_lib.py @@ -78,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)) diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py index 80c1af53..eb65fa45 100755 --- a/mir_restapi/mir_restapi/mir_restapi_server.py +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -105,7 +105,7 @@ def test_api_connection(self): 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(): @@ -159,19 +159,19 @@ def get_sounds_callback(self, 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): From 2db27a047f9da5d5aaa80bdad40b523b40c2d39c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=B6nke=20Niemann?= Date: Thu, 30 Jun 2022 17:41:30 +0200 Subject: [PATCH 75/75] fixes python setuptools error --- mir_driver/setup.cfg | 4 ++-- mir_restapi/setup.cfg | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/mir_driver/setup.cfg b/mir_driver/setup.cfg index b6732ca1..a1cfb96d 100644 --- a/mir_driver/setup.cfg +++ b/mir_driver/setup.cfg @@ -1,4 +1,4 @@ [develop] -script-dir=$base/lib/mir_driver +script_dir=$base/lib/mir_driver [install] -install-scripts=$base/lib/mir_driver \ No newline at end of file +install_scripts=$base/lib/mir_driver \ No newline at end of file diff --git a/mir_restapi/setup.cfg b/mir_restapi/setup.cfg index a9845014..22a79fde 100644 --- a/mir_restapi/setup.cfg +++ b/mir_restapi/setup.cfg @@ -1,4 +1,4 @@ [develop] -script-dir=$base/lib/mir_restapi +script_dir=$base/lib/mir_restapi [install] -install-scripts=$base/lib/mir_restapi +install_scripts=$base/lib/mir_restapi