diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md new file mode 100644 index 0000000..1af794e --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -0,0 +1,30 @@ +--- +name: Bug report +about: Create a report to help us improve +title: '' +labels: 'bug' +assignees: '' + +--- + +**Describe the bug** +A clear and concise description of what the bug is. + +**To Reproduce** +Steps to reproduce the behavior: +1. Code with explanation +2. How to run code +3. The error + +**Expected behavior** +A clear and concise description of what you expected to happen. + +**Screenshots** +If applicable, add screenshots to help explain your problem. + +**Environment (please complete the following information):** + - OS: [e.g. Linux] + - Package versions implicated in the error (torch, mdlearn versions, etc) + +**Additional context** +Add any other context about the problem here. diff --git a/.github/ISSUE_TEMPLATE/feature_request.md b/.github/ISSUE_TEMPLATE/feature_request.md new file mode 100644 index 0000000..36014cd --- /dev/null +++ b/.github/ISSUE_TEMPLATE/feature_request.md @@ -0,0 +1,20 @@ +--- +name: Feature request +about: Suggest an idea for this project +title: '' +labels: 'enhancement' +assignees: '' + +--- + +**Is your feature request related to a problem? Please describe.** +A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] + +**Describe the solution you'd like** +A clear and concise description of what you want to happen. + +**Describe alternatives you've considered** +A clear and concise description of any alternative solutions or features you've considered. + +**Additional context** +Add any other context or screenshots about the feature request here. diff --git a/.github/ISSUE_TEMPLATE/other.md b/.github/ISSUE_TEMPLATE/other.md new file mode 100644 index 0000000..66da7ad --- /dev/null +++ b/.github/ISSUE_TEMPLATE/other.md @@ -0,0 +1,10 @@ +--- +name: Other issue template +about: Any issue that does not fit the other templates +title: '' +labels: '' +assignees: '' + +--- + +**Description** diff --git a/.github/ISSUE_TEMPLATE/question-issue-template.md b/.github/ISSUE_TEMPLATE/question-issue-template.md new file mode 100644 index 0000000..a422d18 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/question-issue-template.md @@ -0,0 +1,10 @@ +--- +name: Question issue template +about: Ask a question about how to use mdlearn +title: '' +labels: question +assignees: '' + +--- + +**Describe the question** diff --git a/.github/workflows/docker-build.yml b/.github/workflows/docker-build.yml deleted file mode 100644 index bcc6674..0000000 --- a/.github/workflows/docker-build.yml +++ /dev/null @@ -1,76 +0,0 @@ -name: Docker Build Only - -# This workflow uses actions that are not certified by GitHub. -# They are provided by a third-party and are governed by -# separate terms of service, privacy policy, and support -# documentation. - -on: - push: - branches: [ "!main" ] - pull_request: - branches: [ "*" ] - - -env: - # Use docker.io for Docker Hub if empty - REGISTRY: ghcr.io - # github.repository as / - IMAGE_NAME: ${{ github.repository }} - - -jobs: - docker_build: - - runs-on: ubuntu-latest - permissions: - contents: read - packages: write - - steps: - - name: Checkout repository - uses: actions/checkout@v3 - - - name: Set up QEMU - uses: docker/setup-qemu-action@v2 - - - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v2 - - # Login against a Docker registry except on PR - # https://github.com/docker/login-action - - name: Log into registry ${{ env.REGISTRY }} - if: github.event_name != 'pull_request' - uses: docker/login-action@343f7c4344506bcbf9b4de18042ae17996df046d # v3.0.0 - with: - registry: ${{ env.REGISTRY }} - username: ${{ github.actor }} - password: ${{ secrets.GITHUB_TOKEN }} - - # Extract metadata (tags, labels) for Docker - # https://github.com/docker/metadata-action - - name: Extract Docker metadata - id: meta - uses: docker/metadata-action@96383f45573cb7f253c731d3b3ab81c87ef81934 # v5.0.0 - with: - images: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }} - tags: | - type=schedule - type=ref,event=branch - type=ref,event=tag - type=ref,event=pr - type=raw,value=latest,enable={{is_default_branch}} - - # Build and push Docker image with Buildx (don't push on PR) - # https://github.com/docker/build-push-action - - name: Build Docker image - id: build-and-push - uses: docker/build-push-action@0565240e2d4ab88bba5387d719585280857ece09 # v5.0.0 - with: - context: . - push: false - tags: ${{ steps.meta.outputs.tags }} - labels: ${{ steps.meta.outputs.labels }} - platforms: linux/amd64,linux/arm64 - #cache-from: type=gha - #cache-to: type=gha,mode=max diff --git a/.github/workflows/docker-publish.yml b/.github/workflows/docker-publish.yml index d26b14f..2336eea 100644 --- a/.github/workflows/docker-publish.yml +++ b/.github/workflows/docker-publish.yml @@ -7,9 +7,11 @@ name: Docker Build and Publish on: push: - branches: [ "main" ] + branches: [ "*" ] # Publish semver tags as releases. tags: [ '*' ] + schedule: # Run on Tuesday's at noon + - cron: '0 12 * * 2' env: # Use docker.io for Docker Hub if empty @@ -20,6 +22,7 @@ env: jobs: build_and_publish: + runs-on: ubuntu-latest permissions: contents: read @@ -35,10 +38,9 @@ jobs: - name: Set up Docker Buildx uses: docker/setup-buildx-action@v2 - # Login against a Docker registry except on PR + # Login against a Docker registry # https://github.com/docker/login-action - name: Log into registry ${{ env.REGISTRY }} - if: github.event_name != 'pull_request' uses: docker/login-action@343f7c4344506bcbf9b4de18042ae17996df046d # v3.0.0 with: registry: ${{ env.REGISTRY }} @@ -59,7 +61,7 @@ jobs: type=ref,event=pr type=raw,value=latest,enable={{is_default_branch}} - # Build and push Docker image with Buildx (don't push on PR) + # Build and push Docker image with Buildx # https://github.com/docker/build-push-action - name: Build and push Docker image id: build-and-push diff --git a/Makefile b/Makefile index 53f3fd0..4fb9960 100644 --- a/Makefile +++ b/Makefile @@ -1,14 +1,9 @@ -# Python Configuration -PYPROJECT_TOML := pyproject.toml -PROJECT_VERSION := $(shell grep -oP '(?<=version = ")[^"]+' $(PYPROJECT_TOML) | head -n 1) - .DEFAULT_GOAL := init -.PHONY += init paths checks test hardware_test clean +.PHONY += init paths checks test clean + init: # Do the initial configuration of the project @test -e .env || cp example.env .env - @sed -i 's/^PROJECT_VERSION=.*/PROJECT_VERSION=$(PROJECT_VERSION)/' .env - @sed -i 's/^PROJECT_PATH=.*/PROJECT_PATH=$(shell pwd | sed 's/\//\\\//g')/' .env .env: init @@ -22,13 +17,5 @@ checks: # Runs all the pre-commit checks test: init .env paths # Runs all the tests @docker compose -f wei.compose.yaml --env-file .env up --build -d - @docker compose -f wei.compose.yaml --env-file .env exec mir_module pytest -p no:cacheprovider -m "not hardware" mir_module + @docker compose -f wei.compose.yaml --env-file .env exec mir_module pytest -p no:cacheprovider mir_module @docker compose -f wei.compose.yaml --env-file .env down - -# hardware_test: init .env paths # Runs all the tests -# @docker compose -f wei.compose.yaml --env-file .env up --build -d -# @docker compose -f wei.compose.yaml --env-file .env exec mir_module pytest -p no:cacheprovider -m "hardware" mir_module -# @docker compose -f wei.compose.yaml --env-file .env down - -clean: - @rm .env diff --git a/README.md b/README.md index f9f646c..55f4559 100644 --- a/README.md +++ b/README.md @@ -1 +1,23 @@ -# mir_module \ No newline at end of file +# MiR Base 250 Module + +## Overview + +The 'mir_module' repository is a comprehensive package that includes a MiR driver and REST node compatible with Mobile Industrial Robots (MiR) MiR 250 Base robot. This module is designed to provide a comprehensive and intuitive approach to controlling and interacting with a MiR 250. The repository allows for both immediate, simplified control of the MiR along with higher-level development of complex missions and experiments. + +## Features + +### MiR 250 Driver Package + +The MiR 250 driver package in this repository supports various methods to enable complete and comprehensive control of your MiR base robot and development of both simple and complex missions and environments. Key focuses and functions of the driver include: + +1. **Debugging and Development** + - Methods such as *get_actions* and *get_action_type* provide detailed information on the types of actions available to the MiR and the necessary parameters for continued development. + +2. **Mission Queue Configuration and Monitoring** + - Upon initialization of your MiR_Base robot, a new mission "session" is started. Functions related to mission queues will refer only to missions started during the session. The session can be reset after initialization as desired. + +3. **High-Level, Multi-Action Mission Development** + +4. **Simplified and Immediate MiR Control** + +... WIP diff --git a/compose.yaml b/compose.yaml index bd7ad2f..f64e2c0 100644 --- a/compose.yaml +++ b/compose.yaml @@ -8,12 +8,11 @@ services: dockerfile: Dockerfile tags: - ${IMAGE}:latest - - ${IMAGE}:${PROJECT_VERSION} - ${IMAGE}:dev - command: python -m mir_rest_node --port 3000 --alias ${MIR_ALIAS} --mir_url ${MIR_URL} --mir_key ${MIR_KEY} + command: python -m mir_rest_node --port 3043 --mir_host ${MIR_HOST} --map_name ${MAP_NAME} env_file: .env volumes: - ./src:/home/app/mir_module/src - ./tests:/home/app/mir_module/tests ports: - - 3000:3000 + - 3043:3043 diff --git a/example.env b/example.env index 31d2bfc..a39a284 100644 --- a/example.env +++ b/example.env @@ -1,11 +1,8 @@ # Note: all paths are relative to the docker compose file -MIR_ALIAS="mir250_base_1" -MIR_URL="146.137.240.35" -MIR_KEY=10100 -PROJECT_PATH= -PROJECT_VERSION= +MIR_HOST="mirbase2.cels.anl.gov" +MAP_NAME="RPL" WEI_DATA_DIR=~/.wei WORKCELL_FILENAME=test_workcell.yaml -WORKCELLS_DIR=${PROJECT_PATH}/tests/workcell_defs +WORKCELLS_DIR=./tests/workcell_defs IMAGE=ghcr.io/ad-sdl/mir_module REDIS_DIR=~/.wei/redis diff --git a/locations.json b/locations.json new file mode 100644 index 0000000..ac05e2f --- /dev/null +++ b/locations.json @@ -0,0 +1,46 @@ +{ + "RPL": { + "camera_marker": { + "pos_x": 8.209, + "guid": "4ccacd0d-7f46-11ee-8521-0001297b4d50", + "orientation": 180.0, + "pos_y": 16.306, + "type_id": 9 + }, + "charger1": { + "pos_x": 10.119, + "guid": "f0908191-7f46-11ee-8521-0001297b4d50", + "orientation": 88.936, + "pos_y": 21.557, + "type_id": 20 + }, + "N9": { + "pos_x": 11.0, + "guid": "7224a20f-d65c-11ee-a833-0001297b4d50", + "orientation": 0.0, + "pos_y": 16.372, + "type_id": 11 + }, + "test_pos": { + "pos_x": 10.119, + "guid": "88c7d8ff-54d0-11ef-90e0-0001297b4d50", + "orientation": 88.936, + "pos_y": 19.0, + "type_id": 0 + }, + "test_move": { + "pos_x": 7.0, + "guid": "d99494c0-54d5-11ef-be3f-0001297b4d50", + "orientation": 180.0, + "pos_y": 20.2, + "type_id": 0 + }, + "another_move": { + "pos_x": 8.1, + "guid": "b34d6e54-5670-11ef-a572-0001297b4d50", + "orientation": 3.18, + "pos_y": 20.4, + "type_id": 0 + } + } +} diff --git a/pyproject.toml b/pyproject.toml index 4156ea0..8e809b8 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -5,6 +5,11 @@ description = "Software for automatting a mir robot" authors = [ {name = "Ryan D. Lewis", email="ryan.lewis@anl.gov"}, {name = "Rafael Vescovi", email="ravescovi@anl.gov"}, + {name = "Doga Ozgulbas", email="dozgulbas@anl.gov"}, + {name = "Abe Stroka", email="astroka@anl.gov"}, + {name = "Kyle Hippe", email = "khippe@anl.gov"}, + {name = "Tobias Ginsburg", email = "tginsburg@anl.gov"}, + {name = "Aileen Cleary", email = "acleary@anl.gov"}, ] dependencies = [ "ad_sdl.wei>=0.5.3", @@ -60,7 +65,7 @@ exclude = [ ] # Same as Black. -line-length = 88 +line-length = 120 indent-width = 4 # Assume Python 3.8 diff --git a/src/mir_driver/locations.json b/src/mir_driver/locations.json new file mode 100644 index 0000000..ac05e2f --- /dev/null +++ b/src/mir_driver/locations.json @@ -0,0 +1,46 @@ +{ + "RPL": { + "camera_marker": { + "pos_x": 8.209, + "guid": "4ccacd0d-7f46-11ee-8521-0001297b4d50", + "orientation": 180.0, + "pos_y": 16.306, + "type_id": 9 + }, + "charger1": { + "pos_x": 10.119, + "guid": "f0908191-7f46-11ee-8521-0001297b4d50", + "orientation": 88.936, + "pos_y": 21.557, + "type_id": 20 + }, + "N9": { + "pos_x": 11.0, + "guid": "7224a20f-d65c-11ee-a833-0001297b4d50", + "orientation": 0.0, + "pos_y": 16.372, + "type_id": 11 + }, + "test_pos": { + "pos_x": 10.119, + "guid": "88c7d8ff-54d0-11ef-90e0-0001297b4d50", + "orientation": 88.936, + "pos_y": 19.0, + "type_id": 0 + }, + "test_move": { + "pos_x": 7.0, + "guid": "d99494c0-54d5-11ef-be3f-0001297b4d50", + "orientation": 180.0, + "pos_y": 20.2, + "type_id": 0 + }, + "another_move": { + "pos_x": 8.1, + "guid": "b34d6e54-5670-11ef-a572-0001297b4d50", + "orientation": 3.18, + "pos_y": 20.4, + "type_id": 0 + } + } +} diff --git a/src/mir_driver/mir_driver.py b/src/mir_driver/mir_driver.py index 12f59a9..807b9f3 100644 --- a/src/mir_driver/mir_driver.py +++ b/src/mir_driver/mir_driver.py @@ -1,15 +1,15 @@ #!/usr/bin/env python3 -"""Driver code for the MiR 250 Robotic base.""" +""" +Driver code for the MiR 250 Robotic base. +""" - -import requests +import datetime as dt import json -import cmd -import json - -from requests.api import post +import time from pprint import pprint +import requests + class MiR_Base: """Main Driver Class for the MiR Robotic base.""" @@ -23,21 +23,22 @@ def __init__( action_dict=None, position_dict=None, curr_mission_queue_id=None, - filename=None + filename="locations.json", + status=None, ): """ - Description: + Initialize the MiRBase class with default or provided values. """ self.mir_ip = mir_ip self.mir_key = mir_key - self.host = "http://" + self.mir_ip + "/api/v2.0.0/" + self.host = f"http://{self.mir_ip}/api/v2.0.0/" - # Formatting the headers. - self.headers = {} - self.headers["Content-Type"] = "application/json" - self.headers["Authorization"] = self.mir_key + # Set up the request headers + self.headers = { + "Content-Type": "application/json", + "Authorization": self.mir_key, + } - ## self.filename = filename self.map_name = map_name self.current_map = self.get_map() @@ -46,646 +47,709 @@ def __init__( self.action_dict = self.create_action_dict() self.position_dict = self.create_position_dict() self.curr_mission_queue_id = self.set_mission_queue_id() + self.status = self.get_state() - def get_map(self): # Works, not refined. + def get_map(self): + """ + Retrieve the current map for the MiR base. + If the map name is not set, the first map from the list will be used. + Otherwise, it tries to match the map name with the available maps. + If no match is found, it defaults to the first map. + + Returns: + dict: The map data of the current map. + """ maps = self.receive_response("maps") - - if not self.map_name: + if not self.map_name: print("Current map not set, using first instance...") - + current_map = maps[0] else: - - current_map = list(filter(lambda map: map['name'] == self.map_name, maps)) - + current_map = list(filter(lambda map: map["name"] == self.map_name, maps)) if not current_map: - current_map = maps[0] - - print("Current Map: ", current_map[0]) + print("Current Map: ", current_map[0]) return current_map[0] - - def get_actions(self, printq=False): # Works, refined. + def get_actions(self, printq=False): """ - get_actions: Retrieves and prints all valid action types and their descriptions. + Retrieve all valid action types and their descriptions. + + Args: + printq (bool): If True, print the retrieved actions. Defaults to False. + + Returns: + list: A list of actions available on the MiR base. """ actions = self.receive_response("actions", printq) - return actions - def get_action_type(self, action_type=str, printq=False): # Works, refined. + def get_action_type(self, action_type=str, printq=False): """ - get_action_type: Retrieves and prints action parameters for a given action type. + Retrieve and print action parameters for a given action type. + + Args: + action_type (str): The type of action to retrieve. + printq (bool): If True, print the action parameters. Defaults to False. + + Returns: + dict: The parameters for the specified action type. """ - url = "actions/" + action_type + url = f"actions/{action_type}" action_params = self.receive_response(url, printq) - return action_params - - def list_missions(self, printq=False): # Works, refined. - ''' - list_mission : Lists all created missions for the MiR. - ''' - all_missions = self.receive_response("missions", printq) + def list_missions(self, printq=False): + """ + List all created missions for the MiR base. + + Args: + printq (bool): If True, print the list of missions. Defaults to False. + + Returns: + list: A list of all missions created for the MiR base. + """ + all_missions = self.receive_response("missions", printq) return all_missions - - def get_mission_queue(self, printq=False): # Works, refined. - ''' - get_mission_queue : Prints all missions in the queue since the last mission queue id. - ''' - search = { - "filters" : [{ - "fieldname" : "id", - "operator" : ">", - "value" : self.curr_mission_queue_id - }]} - - mission_queue = self.receive_response("mission_queue", printq, None, True, search) - if len(mission_queue) < 1: + + def get_mission_queue(self, printq=False): + """ + Retrieve all missions in the queue since the last mission queue ID. + + Args: + printq (bool): If True, print the list of missions in the queue. Defaults to False. + + Returns: + list: A list of missions posted to the queue since the last session. + Returns None if no missions were found. + """ + search = {"filters": [{"fieldname": "id", "operator": ">", "value": self.curr_mission_queue_id}]} + mission_queue = self.receive_response("mission_queue", printq, None, search) + + if not mission_queue: print("No missions posted to queue since last session.") - return + return None return mission_queue - - def abort_mission_queue(self): # Works, refined. - ''' - abort_mission_queue : Aborts all pending and executing missions in the mission queue. - ''' + + def abort_mission_queue(self): + """ + Abort all pending and executing missions in the mission queue. + + Returns: + dict: The response from the MiR base after aborting the missions. + """ response = self.delete("mission_queue", False, "All missions aborted.") - return response - + def clear_mission_queue(self): + """ + Clear the current mission queue by setting a new mission queue ID. + Returns: + str: The new mission queue ID. + """ self.curr_mission_queue_id = self.set_mission_queue_id() - return self.curr_mission_queue_id - - def find_mission_in_queue(self, mission_name): # Works, refined. - ''' - find_mission_in_queue : Prints mission and action details for given mission name in queue. - ''' - search = {"filters" : [{"fieldname" : "name","operator" : "=","value" : mission_name}]} + + def find_mission_in_queue(self, mission_name): + """ + Find and print mission and action details for the given mission name in the queue. + + Args: + mission_name (str): The name of the mission to search for. + + Returns: + None + """ + search = {"filters": [{"fieldname": "name", "operator": "=", "value": mission_name}]} mission = self.receive_response("missions", False, None, search) - if len(mission) < 1: + if not mission: print("No existing mission found under that name.") return - - mission_id = mission[0].get("guid") - search = {"filters" : [{"fieldname" : "mission_id","operator" : "=","value" : mission_id},{"fieldname" : "id","operator" : ">","value" : self.curr_mission_queue_id}]} - - mission = self.receive_response("mission_queue", False, None, search) - if len(mission) < 1: + mission_guid = mission[0].get("guid") + search = { + "filters": [ + {"fieldname": "mission_id", "operator": "=", "value": mission_guid}, + {"fieldname": "id", "operator": ">", "value": self.curr_mission_queue_id}, + ] + } + + mission_queue = self.receive_response("mission_queue", False, None, search) + + if not mission_queue: print("No existing mission found under that name in the current queue.") return - - mission_id = mission[0].get("id") - - url = "mission_queue/" + str(mission_id) - mission_details = self.receive_response(url, True, "Mission details: ") - url = "mission_queue/" + str(mission_id) + "/actions" - action_details = self.receive_response(url, True, "Action details: ") + mission_id = mission_queue[0].get("id") + + url = f"mission_queue/{mission_id}" + self.receive_response(url, True, "Mission details: ") + + url = f"missions/{mission_guid}/actions" + self.receive_response(url, True, "Action details: ") return - + def cancel_mission_in_queue(self, mission_name): - ''' - cancel_mission_in_queue : Aborts given mission name from current queue if found. - ''' - search = {"filters" : [{"fieldname" : "name", "operator" : "=", "value" : mission_name}]} + """ + Abort the given mission name from the current queue if found. + + Args: + mission_name (str): The name of the mission to search and cancel. + + Returns: + None + """ + search = {"filters": [{"fieldname": "name", "operator": "=", "value": mission_name}]} mission = self.receive_response("missions", False, None, search) - if len(mission) < 1: + if not mission: print("No existing mission found under that name.") return - + mission_id = mission[0].get("guid") - search = {"filters" : [{"fieldname" : "mission_id","operator" : "=","value" : mission_id},{"fieldname" : "id","operator" : ">","value" : self.curr_mission_queue_id}]} - - mission = self.receive_response("mission_queue", False, None, search) + search = { + "filters": [ + {"fieldname": "mission_id", "operator": "=", "value": mission_id}, + {"fieldname": "id", "operator": ">", "value": self.curr_mission_queue_id}, + ] + } - if len(mission) < 1: + mission_queue = self.receive_response("mission_queue", False, None, search) + + if not mission_queue: print("No existing mission found under that name in the current queue.") return - - mission_id = mission[0].get("id") - - url = "mission_queue/" + str(mission_id) - response = self.delete(url) - + + mission_id = mission_queue[0].get("id") + + url = f"mission_queue/{mission_id}" + self.delete(url) + return - - def find_act_type(self, action_type): # Works. - ''' - find_act_type : Returns parameter details for a given action type. - ''' - - actions = self.action_dict - parameters = actions.get(action_type)["parameters"] - + + def find_act_type(self, action_type): + """ + Retrieve parameter details for a given action type. + + Args: + action_type (str): The type of action for which to retrieve parameters. + + Returns: + dict: A dictionary of parameters for the specified action type. + """ + parameters = self.action_dict.get(action_type, {}).get("parameters", {}) return parameters - + def init_mission(self, mission_name, description, printq=False): - ''' - init_mission : Helper function initializing new mission. - ''' + """ + Initialize a new mission with the given name and description. - Missions = {"description" : description,"group_id" : self.group_id,"name" : mission_name} + Args: + mission_name (str): The name of the new mission. + description (str): A description of the new mission. + printq (bool): If True, print the response. Defaults to False. - response = self.send_command("missions", Missions, printq, "New mission successfully added") + Returns: + dict: The response from the MiR base after initializing the mission. + """ + + mission_data = {"description": description, "group_id": self.group_id, "name": mission_name} + response = self.send_command("missions", mission_data, printq, "New mission successfully added") return response - + def init_action(self, act_param_dict, mission_id, priority, printq=False): - ''' - init_action : Helper function initializing actions with default values when creating a new mission. - ''' - - for i in range(len(act_param_dict)): + """ + Initialize actions with default values for a new mission. + + Args: + act_param_dict (list of dict): List of dictionaries where each dictionary contains action types and their parameters. + mission_id (str): The ID of the mission to which actions are being added. + priority (int): Priority level for the actions. + printq (bool): If True, print the response. Defaults to False. - action_type = list(act_param_dict[i].keys())[0] + Returns: + None + """ + for action_params in act_param_dict: + action_type = list(action_params.keys())[0] parameters = self.find_act_type(action_type) action_payload = { - "action_type" : action_type, - "parameters" : parameters, - "id" : mission_id, - "priority" : priority - } - - url = "missions/" + mission_id + "/actions" - response = self.send_command(url, action_payload, printq, "New action successfully added.") - - return - + "action_type": action_type, + "parameters": parameters, + "id": mission_id, + "priority": priority, + } + + url = f"missions/{mission_id}/actions" + self.send_command(url, action_payload, printq, "New action successfully added.") + + return + def set_action_params(self, mission_id, act_param_dict, printq): - ''' - set_action_params : Helper function to modify action parameters for a mission. - ''' + """ + Modify action parameters for a mission. - url = "missions/" + mission_id + "/actions" + Args: + mission_id (str): The ID of the mission to modify actions for. + act_param_dict (list of dict): List of dictionaries where each dictionary contains action types and their updated parameters. + printq (bool): If True, print the response. Defaults to False. + + Returns: + None + """ + url = f"missions/{mission_id}/actions" actions = self.receive_response(url, printq) for action in actions: - - params = action.get("parameters") + params = action.get("parameters", []) action_type = action.get("action_type") action_id = action.get("guid") if action_type != list(act_param_dict[0].keys())[0]: - raise ValueError("Wrong") + raise ValueError("Action type mismatch.") + + cur_dict = act_param_dict.pop(0).get(action_type, {}) - cur_dict = act_param_dict.pop(0).get(action_type) - for k, v in cur_dict.items(): for param in params: - if param['id'] == k or param['input_name'] == k: - param['value'] = v - - PutMission_action = {"parameters" : params,"priority" : 1,"scope_reference" : None} - - url = "missions/" + mission_id + "/actions/" + action_id - change_action = self.change_command(url, PutMission_action, printq, "Action successfully changed.") - - return - - - def post_mission_to_queue(self, mission_name, act_param_dict, description="", priority=1, printq=False): - ''' - post_mission_to_queue: Checks if the mission name exists, if not, initializes the mission and actions with default values, then modifies the actions with the new parameters - and posts the mission to queue. - 'mission_name' : Name to post mission under. If unique, creates new mission, if not, modifies existing mission. - 'act_param_dict' : List of dictionaries, where each dictionary takes the form of: - { : - {, - ... - }} - Actions must be given in the same order as when the mission was created. Only include parameter ids that you wish to change, others do not have to be specified. - 'description' : Enter a description for the mission if desired, otherwise blank. - 'priority' : Assign a priority when posting the mission to queue, otherwise 1. - 'printq' : Print all mission and action details throughout the process. - ''' - - search = {"filters" : [{"fieldname" : "name","operator" : "=","value" : mission_name}]} + if param["id"] == k or param["input_name"] == k: + param["value"] = v + + mission_actions = {"parameters": params, "priority": 1, "scope_reference": None} + + url = f"missions/{mission_id}/actions/{action_id}" + self.change_command(url, mission_actions, printq, "Action successfully changed.") + + return + + def post_mission_to_queue(self, mission_name, act_param_dict, description="", priority=0, printq=False): + """ + Post a mission to the queue. Creates a new mission if it doesn't exist, otherwise updates the existing mission. + Initializes and modifies actions as specified and adds the mission to the queue. + + Args: + mission_name (str): The name of the mission. + act_param_dict (list of dict): List of dictionaries where each dictionary contains action types and their updated parameters. + description (str): Description of the mission. Defaults to an empty string. + priority (int): Priority level when posting the mission to the queue. Defaults to 1. + printq (bool): If True, print the response. Defaults to False. + + Returns: + dict: Response from the MiR base after posting the mission to the queue. + """ + search = {"filters": [{"fieldname": "name", "operator": "=", "value": mission_name}]} mission = self.receive_response("missions", False, None, search) - if len(mission) < 1: - + if not mission: mission = self.init_mission(mission_name, description, printq) - mission_id = mission.get("guid") - - actions = self.init_action(act_param_dict, mission_id, priority, printq) + self.init_action(act_param_dict, mission_id, priority, printq) else: mission_id = mission[0].get("guid") - params = self.set_action_params(mission_id, act_param_dict, printq) + self.set_action_params(mission_id, act_param_dict, printq) - Mission_queues = {"mission_id" : mission_id,"priority" : priority} - - response = self.send_command("mission_queue", Mission_queues, printq, "Mission successfully added to queue.") - - return response + mission_queue_payload = {"mission_id": mission_id, "priority": priority} + response = self.send_command( + "mission_queue", mission_queue_payload, printq, "Mission successfully added to queue." + ) - def check_queue_completion(self): + return response - mission_queue = self.get_mission_queue() - width = 50 + def wait_until_finished(self): + """ + Continuously checks previously index mission to be done executing. Breaks out of loop once state is achieved. + Prevents further missions or actions being sent if desired, since "Executing" != BUSY. + """ + self.status = "BUSY" + mission_finished = self.get_mission_queue()[-1]["state"] + while mission_finished != "Done": + mission_finished = self.get_mission_queue()[-1]["state"] + time.sleep(5) + self.status = "IDLE" + return - print("Current Mission Queue: \n") - for i in range(len(mission_queue)): - print(mission_queue[i]["name"] + ": " + mission_queue[i]["state"] + "\n") - if mission_queue[i]["state"] == "Pending": - cur_mission = mission_queue[i] - index = i + 1 + def check_queue_completion(self, printq=False): + """ + Check and print the status of the current mission queue and its actions. - percent = (index/len(mission_queue)) * 100 - bar_length = int(width*(index/len(mission_queue))) - bar = '#' * bar_length + '-' * (width - bar_length) - print(f"\r[{bar}] {percent:6.2f}% Queue Complete\n") + This function retrieves the current mission queue, displays the completion status of the queue, + and then retrieves and displays the actions for the current mission, showing their completion status. - print("Current Mission: " + cur_mission["name"] + "\n") + Returns: + None + """ + mission_queue = self.get_mission_queue() - action_queue = requests.get( - self.host + "mission_queue/" + cur_mission["guid"] + "/actions", - headers=self.headers - ) + if not mission_queue: + print("No missions in the queue.") + return - action_details = action_queue.text + width = 50 + current_mission = [m for m in mission_queue if m["state"] == "Executing"] - print("Current Mission Actions: \n") - for i in range(len(action_details)): - print(action_details[i]["action_type"] + ": " + action_details[i]["state"] + "\n") - if action_details[i]["state"] == "Pending": - cur_action = action_details[i] - index = i + 1 + if current_mission: + index = mission_queue.index(current_mission[0]) + percent = (index / len(mission_queue)) * 100 + bar_length = int(width * (index / len(mission_queue))) + bar = "#" * bar_length + "-" * (width - bar_length) - percent = (index/len(action_details)) * 100 - bar_length = int(width*(index/len(action_details))) - bar = '#' * bar_length + '-' * (width - bar_length) - print(f"\r[{bar}] {percent:6.2f}% Mission Complete\n") + miss_id = current_mission[0].get("id") + mission_guid = self.receive_response("mission_queue/" + str(miss_id)).get("mission_id") + mission_name = self.receive_response("missions/" + mission_guid).get("name") - print("Current Action: " + cur_action["action_type"] + "\n") + print(f"\r[{bar}] {percent:6.2f}% Queue Complete\n") + print(f"Current Mission: {mission_name}\n") + if printq: + self.find_mission_in_queue(mission_name) return - def status(self): - ''' - status : Retrieves MiR system status. - ''' - self_status = self.receive_response("status", True) + def self_status(self): + """ + Retrieves the current system status of the MiR Robotic base. + Returns: + dict: The system status information. + """ + self_status = self.receive_response("status", True) return self_status - - def get_mission_actions_by_index(self, index): - ''' - get_mission_actions_by_index : Retrieves mission and corresponding action details for the mission at the given index, for debugging. - ''' - - mission_details = self.receive_response("missions", False) - mission_id = mission_details[index].get("guid") - - url = "missions/" + mission_id + "/actions" - actions = self.receive_response(url, True) - return actions - def get_user_group_id(self): - ''' - get_user_group_id : Gets first mission group id, necessary when posting/creating a mission. Can be modified if different mission group desired. - ''' + """ + Retrieves the ID of the first mission group. + This ID is necessary when posting or creating a mission. Can be modified if a different mission group is desired. + + Returns: + str: The ID of the first mission group. + """ get_id = self.receive_response("mission_groups", False) id = get_id[0].get("guid") - return id - - def receive_response(self, get, printq=False, message=None, search=None): - - if search!=None: - get = get + "/search" - body = search + def receive_response(self, endpoint, printq=False, message=None, search=None): + """ + Sends a GET or POST request to the MiR API and handles the response. POST requests are modified GET requests with search payloads to filter the response. - response = requests.post( - self.host + get, - json=body, - headers=self.headers - ) + Args: + endpoint (str): The API endpoint to query. + printq (bool): Whether to print the response details. + message (str): An optional message to print if the request is successful. + search (dict): An optional search payload for POST requests. - else: + Returns: + dict: The parsed JSON response from the API. - url = get - response = requests.get( - self.host + url, - headers=self.headers - ) - + Raises: + ValueError: If the API request fails. + """ + if search is not None: + url = f"{endpoint}/search" + response = requests.post(f"{self.host}{url}", json=search, headers=self.headers) + else: + response = requests.get(f"{self.host}{endpoint}", headers=self.headers) text = json.loads(response.text) status = response.status_code - if status == 200 or status == 201: - if message != None: + if status in {200, 201}: + if message is not None: print(message) if printq: pprint(text) else: - raise ValueError("Error sending GET request: ", text, status) - + raise ValueError(f"Error sending GET request: {text} (Status code: {status})") + return text - - def send_command(self, post, body, printq=False, message=None): - - url = post - command = requests.post( - self.host + url, - json=body, - headers=self.headers - ) - text = json.loads(command.text) - status = command.status_code + def send_command(self, endpoint, body, printq=False, message=None): + """ + Sends a POST request to the MiR API and handles the response. + + Args: + endpoint (str): The API endpoint to post data to. + body (dict): The JSON payload to send in the request. + printq (bool): Whether to print the response details. + message (str): An optional message to print if the request is successful. + + Returns: + dict: The parsed JSON response from the API. + + Raises: + ValueError: If the API request fails. + """ + response = requests.post(f"{self.host}{endpoint}", json=body, headers=self.headers) + text = json.loads(response.text) + status = response.status_code if status == 201: - if message != None: + if message is not None: print(message) if printq: pprint(text) else: - raise ValueError("Error sending POST request: ", text, status) - + raise ValueError(f"Error sending POST request: {text} (Status code: {status})") + return text - - def change_command(self, put, body, printq=False, message=None): - - url = put - - change = requests.put( - self.host + url, - json=body, - headers=self.headers - ) - if printq: - print("URL: ", url) - print("BODY: ", body) - print("RESPONSE: ", change) - text = json.loads(change.text) - status = change.status_code + def change_command(self, endpoint, body, printq=False, message=None): + """ + Sends a PUT request to the MiR API to update data and handles the response. + + Args: + endpoint (str): The API endpoint to update data at. + body (dict): The JSON payload to send in the request. + printq (bool): Whether to print the response details. + message (str): An optional message to print if the request is successful. + + Returns: + dict: The parsed JSON response from the API. - if status == 200 or status == 201: - if message != None: + Raises: + ValueError: If the API request fails. + """ + response = requests.put(f"{self.host}{endpoint}", json=body, headers=self.headers) + text = json.loads(response.text) + status = response.status_code + + if status in {200, 201}: + if message is not None: print(message) if printq: pprint(text) else: - raise ValueError("Error sending PUT request: ", text, status) - + raise ValueError(f"Error sending PUT request: {text} (Status code: {status})") + return text - - def delete(self, delete, printq=False, message=None): - url = delete - response = requests.delete( - self.host + url, - headers=self.headers - ) + def delete(self, endpoint, printq=False, message=None): + """ + Sends a DELETE request to the MiR API and handles the response. + + Args: + endpoint (str): The API endpoint to delete data from. + printq (bool): Whether to print the response details. + message (str): An optional message to print if the request is successful. + Returns: + str: The response text from the API. + + Raises: + ValueError: If the API request fails. + """ + response = requests.delete(f"{self.host}{endpoint}", headers=self.headers) text = response.text status = response.status_code if status == 204: - if message != None: + if message is not None: print(message) if printq: pprint(text) else: - raise ValueError("Error sending DELETE request: ", text, status) - + raise ValueError(f"Error sending DELETE request: {text} (Status code: {status})") + return text - + def create_action_dict(self): - ''' - create_action_dict : Dictionary setting default parameters for useful action types. - 'id' : Parameter name, must be used when changing default value. - 'input_name' : Required parameter, set to None, otherwise will create a variable that cannot be changed/stacked with multiple actions. - 'value' : Contains default value for all parameters, remains default unless modified explicitly when posting a mission. - ''' + """ + Creates a dictionary with default parameters for various action types used in missions. + Returns: + dict: A dictionary containing default parameter values for action types such as 'relative_move', 'move_to_position', 'move', and 'docking'. + """ action_dict = { - "relative_move" : { - "parameters" : [ - { - 'id' : 'x', - 'input_name' : None, - 'value' : 0.0 - }, - { - 'id' : 'y', - 'input_name' : None, - 'value' : 0.0 - }, - { - 'id' : 'orientation', - 'input_name' : None, - 'value' : 0.0 - }, - { - 'id' : 'max_linear_speed', - 'input_name' : None, - 'value' : 0.25 - }, - { - 'id' : 'max_angular_speed', - 'input_name' : None, - 'value' : 0.25 - }, - { - 'id' : 'collision_detection', - 'input_name' : None, - 'value' : True - } + "relative_move": { + "parameters": [ + {"id": "x", "input_name": None, "value": 0.0}, + {"id": "y", "input_name": None, "value": 0.0}, + {"id": "orientation", "input_name": None, "value": 0.0}, + {"id": "max_linear_speed", "input_name": None, "value": 0.25}, + {"id": "max_angular_speed", "input_name": None, "value": 0.25}, + {"id": "collision_detection", "input_name": None, "value": True}, ] }, - "move_to_position" : { - "parameters" : [ - { - 'id' : 'x', - 'input_name' : None, - 'value' : 0.0 - }, - { - 'id' : 'y', - 'input_name' : None, - 'value' : 0.0 - }, - { - 'id' : 'orientation', - 'input_name' : None, - 'value' : 0.0 - }, - { - 'id' : 'retries', - 'input_name' : None, - 'value' : 10 - }, - { - 'id' : 'distance_threshold', - 'input_name' : None, - 'value' : 0.1 - } + "move_to_position": { + "parameters": [ + {"id": "x", "input_name": None, "value": 0.0}, + {"id": "y", "input_name": None, "value": 0.0}, + {"id": "orientation", "input_name": None, "value": 0.0}, + {"id": "retries", "input_name": None, "value": 10}, + {"id": "distance_threshold", "input_name": None, "value": 0.1}, ] }, - "move" : { - "parameters" : [ - { - 'id' : 'position', - 'input_name' : None, - 'name': 'another_move', - 'value': 'b34d6e54-5670-11ef-a572-0001297b4d50' - }, + "move": { + "parameters": [ { - 'id' : 'cart_entry_position', - 'input_name' : None, - 'name' : 'Main', - 'value' : 'main' + "id": "position", + "input_name": None, + "name": "another_move", + "value": "b34d6e54-5670-11ef-a572-0001297b4d50", }, - { - 'id' : 'main_or_entry_position', - 'input_name' : None, - 'name' : 'Main', - 'value' : 'main' - }, - { - 'id' : 'marker_entry_position', - 'input_name' : None, - 'name' : 'Entry', - 'value' : 'entry' - }, - { - 'id' : 'retries', - 'input_name' : None, - 'value' : 10 - }, - { - 'id' : 'distance_threshold', - 'input_name' : None, - 'value' : 0.1 - } + {"id": "cart_entry_position", "input_name": None, "name": "Main", "value": "main"}, + {"id": "main_or_entry_position", "input_name": None, "name": "Main", "value": "main"}, + {"id": "marker_entry_position", "input_name": None, "name": "Entry", "value": "entry"}, + {"id": "retries", "input_name": None, "value": 10}, + {"id": "distance_threshold", "input_name": None, "value": 0.1}, ] }, - "docking" : { - "parameters" : [ + "docking": { + "parameters": [ { - 'id' : 'marker', - 'input_name' : None, - 'name' : 'camera_marker', - 'value' : '4ccacd0d-7f46-11ee-8521-0001297b4d50' + "id": "marker", + "input_name": None, + "name": "camera_marker", + "value": "4ccacd0d-7f46-11ee-8521-0001297b4d50", }, { - 'id' : 'marker_type', - 'input_name' : None, - 'name' : 'Narrow asymmetric MiR500/1000 shelf', - 'value' : 'mirconst-guid-0000-0001-marker000001' + "id": "marker_type", + "input_name": None, + "name": "Narrow asymmetric MiR500/1000 shelf", + "value": "mirconst-guid-0000-0001-marker000001", }, - { - 'id' : 'retries', - 'input_name' : None, - 'value' : 10 - }, - { - 'id' : 'max_linear_speed', - 'input_name' : None, - 'value' : 0.1 - } - ] - } + {"id": "retries", "input_name": None, "value": 10}, + {"id": "max_linear_speed", "input_name": None, "value": 0.3}, + ] + }, + "wait": {"parameters": [{"id": "time", "input_name": None, "value": "00:00:05.000000"}]}, } return action_dict def create_position_dict(self): + """ + Creates a dictionary of positions from a map and saves it to a JSON file. + + The dictionary maps position names to their details, excluding those with 'entry' in their names. - url = "maps/" + self.map_guid + "/positions" + Returns: + None + """ + url = f"maps/{self.map_guid}/positions" map_positions = self.receive_response(url) position_dict = {} for position in map_positions: - pos_id = position.get("guid") - url = "positions/" + pos_id + "?whitelist=pos_x,type_id,orientation,guid,pos_y" + type_id = position.get("type_id") + + url = f"positions/{pos_id}?whitelist=pos_x,type_id,orientation,guid,pos_y" filtered = self.receive_response(url) - url = "positions/" + pos_id + "?whitelist=name" + url = f"positions/{pos_id}?whitelist=name" name = self.receive_response(url).get("name") - original_name = name - copy_count = 1 - while name in position_dict: - name = f"{original_name}_copy{copy_count}" - copy_count += 1 + url = f"position_types/{type_id}" + position_type = self.receive_response(url) + pos_name = position_type.get("name") - position_dict[name] = filtered + if "entry" not in pos_name: + position_dict[name] = filtered data = {self.map_name: position_dict} filename = self.filename - with open(filename, 'w') as file: + with open(filename, "w") as file: json.dump(data, file, indent=4) return - def set_mission_queue_id(self): + """ + Gets the ID of the last mission in the mission queue. + Returns: + int: The ID of the last mission in the queue. + """ mission_queue = self.receive_response("mission_queue") last_mission_id = mission_queue[-1].get("id") return last_mission_id - + def get_state(self): + """ + Retrieves the current state of the system. + Returns: + str: The current state of the system. + """ url = "status/?whitelist=state_text" - state = self.receive_response(url, False) - state = state.get("state_text") + state = self.receive_response(url, False).get("state_text") print(state.upper()) return state.upper() - - def move(self, location, mission_name): - f = open(self.filename) - data = json.load(f) - guid = data[self.map_name][location]["guid"] - move =self.post_mission_to_queue(mission_name,[{"move": {"position":guid}}]) + def move(self, location_name): + """ + Creates a mission to move to a specified location and adds it to the mission queue. + + Args: + location (str): The location to move to. + + Returns: + dict: The response from posting the mission to the queue. + """ + mission_name = f"dock_to_{location_name}_{dt.datetime.now()}" + with open(self.filename) as f: + data = json.load(f) + + guid = data[self.map_name][location_name]["guid"] + move = self.post_mission_to_queue(mission_name, [{"move": {"position": guid}}]) return move - - def dock(self, location, mission_name): - f = open(self.filename) - data = json.load(f) - guid = data[self.map_name][location]["guid"] - dock =self.post_mission_to_queue(mission_name,[{"docking": {"marker":guid}}], printq=True) + def dock(self, location_name): + """ + Creates a mission to dock at a specified location and adds it to the mission queue. + + Args: + location (str): The location to dock at. + + Returns: + dict: The response from posting the mission to the queue. + """ + mission_name = f"dock_to_{location_name}_{dt.datetime.now()}" + with open(self.filename) as f: + data = json.load(f) + + guid = data[self.map_name][location_name]["guid"] + dock = self.post_mission_to_queue(mission_name, [{"docking": {"marker": guid}}]) return dock + def wait(self, delay_seconds): + """ + Creates a mission to wait for a specified time and adds it to the mission queue. -if __name__ == "__main__": - mir_base = MiR_Base(map_name="RPL", filename="locations.json") + Args: + time (str): The amount of time to wait for. + + Returns: + dict: The response from posting the mission to the queue. + """ + time = str(dt.timedelta(seconds=delay_seconds)) + mission_name = f"wait_for_{time}_{dt.datetime.now()}" + wait = self.post_mission_to_queue(mission_name, [{"wait": {"time": time}}]) + + return wait + +if __name__ == "__main__": + mir_base = MiR_Base(map_name="RPL") + print(mir_base.get_state()) # response = requests.post( # mir_base.host + "mission_queue/search", # json = { @@ -707,43 +771,46 @@ def dock(self, location, mission_name): # mir_base.post_mission_to_queue("testing_8.14.01" + str(i), [{"move" : {"position" : "d99494c0-54d5-11ef-be3f-0001297b4d50"}}], "testing", 1, True) # mir_base.find_mission_in_queue("testing_8.14.011") - #mir_base.post_mission_to_queue("testing_8.13.008", [{"move" : {"position" : "d99494c0-54d5-11ef-be3f-0001297b4d50"}},{"docking" : {"marker" : "f0908191-7f46-11ee-8521-0001297b4d50"}}], "testing", 1, True) + # mir_base.post_mission_to_queue("testing_8.13.008", [{"move" : {"position" : "d99494c0-54d5-11ef-be3f-0001297b4d50"}},{"docking" : {"marker" : "f0908191-7f46-11ee-8521-0001297b4d50"}}], "testing", 1, True) # response = mir_base.get_info(-1) - #y = mir_base.status() + # y = mir_base.status() ## TESTING: # mir_base.get_actions(True) - # mir_base.get_action_type("move", True) + # mir_base.get_action_type("wait", True) # mir_base.list_missions(True) # mir_base.get_mission_queue(True) # for i in range(5): - # mir_base.post_mission_to_queue("testing_8.14.01" + str(i), [{"move" : {"position" : "d99494c0-54d5-11ef-be3f-0001297b4d50"}}], "testing", 1, False) - # for i in range(10): - # mir_base.get_mission_queue(True) - + # mir_base.dock("charger1") # for i in range(5): - # mir_base.post_mission_to_queue("testing_8.14.01" + str(i), [{"move" : {"position" : "d99494c0-54d5-11ef-be3f-0001297b4d50"}}], "testing", 1, False) + # mir_base.get_mission_queue(True) # mir_base.abort_mission_queue() # mir_base.get_mission_queue(True) - # for i in range(5): - # mir_base.post_mission_to_queue("testing_8.14.01" + str(i), [{"move" : {"position" : "d99494c0-54d5-11ef-be3f-0001297b4d50"}}], "testing", 1, False) - # mir_base.find_mission_in_queue("testing_8.14.011") - - # mir_base.post_mission_to_queue("testing_8.16.021", [{"move" : {"position" : "d99494c0-54d5-11ef-be3f-0001297b4d50"}}], "testing", 1, False) - # mir_base.move("test_move","testing_8.16.022") - # mir_base.post_mission_to_queue("testing_8.16.021", [{"docking" : {"marker" : "f0908191-7f46-11ee-8521-0001297b4d50"}}], "testing", 1, False) - # mir_base.move("charger1", "testing_8.16.022") - - mir_base.dock("charger1", "testing_08.16.030") - - - - + # mission_name = "test_" + str(dt.datetime.now()) + # mir_base.post_mission_to_queue(mission_name, [{"move" : {"position" : "d99494c0-54d5-11ef-be3f-0001297b4d50"}}], "testing", 1, False) + # mir_base.find_mission_in_queue(mission_name) + # mir_base.cancel_mission_in_queue(mission_name) + # mir_base.get_mission_queue(True) - - - + # for i in range(1): + # mir_base.move("test_move") + # mir_base.move("another_move") + # print(mir_base.get_state()) + # print(mir_base.get_state()) + # print(mir_base.get_state()) + # print(mir_base.get_state()) + # time.sleep(20) + # mir_base.check_queue_completion() + # mir_base.abort_mission_queue() + # for _ in range(30): + # mir_base.move("test_pos") + # mir_base.dock("charger1") + # mir_base.wait("60") + + # while mir_base.get_mission_queue() is not None: + # mir_base.check_queue_completion() + # time.sleep(25) diff --git a/src/mir_rest_node.py b/src/mir_rest_node.py index c372820..18bd2f1 100644 --- a/src/mir_rest_node.py +++ b/src/mir_rest_node.py @@ -1,170 +1,108 @@ -#! /usr/bin/env python3 -"""The server for the PF400 robot that takes incoming WEI flow requests from the experiment application""" - -import datetime -import json -import traceback -from argparse import ArgumentParser, Namespace -from contextlib import asynccontextmanager +"""REST-based node for UR robots""" + from pathlib import Path -from time import sleep - -from fastapi import FastAPI -from fastapi.responses import JSONResponse -from wei.core.data_classes import ( - ModuleAbout, - ModuleAction, - ModuleActionArg, -) -from wei.helpers import extract_version - - -def parse_args() -> Namespace: - """Parses the command line arguments for the PF400 REST node""" - parser = ArgumentParser() - parser.add_argument("--alias", type=str, help="Name of the Node", default="pf400") - parser.add_argument("--host", type=str, help="Host for rest", default="0.0.0.0") - parser.add_argument("--port", type=int, help="port value") - return parser.parse_args() - - -global pf400_ip, pf400_port, state, action_start - - -@asynccontextmanager -async def lifespan(app: FastAPI): - """Initial run function for the app, parses the workcell argument - Parameters - ---------- - app : FastApi - The REST API app being initialized - - Returns - ------- - None""" - global pf400, state, pf400_ip, pf400_port - - args = parse_args() - pf400_ip = args.pf400_ip - pf400_port = args.pf400_port - - try: - pf400 = PF400(pf400_ip, pf400_port) - pf400.initialize_robot() - state = "IDLE" - except Exception: - state = "ERROR" - traceback.print_exc() - else: - print("PF400 online") - yield - - # Do any cleanup here - pass - - -app = FastAPI( - lifespan=lifespan, -) - - -def check_state(): - """Updates the MiR state - - Parameters: - ----------- - None - Returns - ------- - None - """ - pass - - -@app.get("/state") -def state(): - """Returns the current state of the Pf400 module""" - global state, action_start - if not (state == "BUSY") or ( - action_start - and (datetime.datetime.now() - action_start > datetime.timedelta(0, 2)) - ): - check_state() - return JSONResponse(content={"State": state}) - - -@app.get("/resources") -async def resources(): - """Returns info about the resources the module has access to""" - global pf400 - return JSONResponse(content={"State": pf400.get_status()}) - - -@app.get("/about") -async def about() -> JSONResponse: - """Returns a description of the actions and resources the module supports""" - global state - about = ModuleAbout( - name="Pf400 Robotic Arm", - model="Precise Automation PF400", - description="pf400 is a robot module that moves plates between two robot locations.", - interface="wei_rest_node", - version=extract_version(Path(__file__).parent.parent / "pyproject.toml"), - actions=[ - ModuleAction( - name="transfer", - description="This action transfers a plate from a source robot location to a target robot location.", - args=[ - ModuleActionArg( - name="source", - description="Source location in the workcell for pf400 to grab plate from.", - type="str", - required=True, - ), - ], - ), - ], - resource_pools=[], - ) - return JSONResponse(content=about.model_dump(mode="json")) - - -@app.post("/action") -def do_action(action_handle: str, action_vars: str): - """Executes the action requested by the user""" - response = {"action_response": "", "action_msg": "", "action_log": ""} - print(action_vars) - global pf400, state, action_start - if state == "BUSY": - return - action_start = datetime.datetime.now() - if state == "PF400 CONNECTION ERROR": - response["action_response"] = "failed" - response["action_log"] = "Connection error, cannot accept a job!" - return response - - vars = json.loads(action_vars) - - err = False - state = "BUSY" - if action_handle == "mission": - print("test_mission") - else: - msg = "UNKNOWN ACTION REQUEST! Available actions: mission" - response["action_response"] = "failed" - response["action_log"] = msg - return response +from typing import List, Optional + +from mir_driver.mir_driver import MiR_Base +from typing_extensions import Annotated +from madsci.common.ownership import get_current_ownership_info +from madsci.common.types.node_types import RestNodeConfig +from madsci.node_module.helpers import action +from madsci.node_module.rest_node_module import RestNode +from madsci.common.types.location_types import LocationArgument + +class MIRConfig(RestNodeConfig): + """Configuration for the MIR REST node""" + mir_host: str = "mirbase2.cels.anl.gov" + map_name: str = "RPL" + +class MIRNode: + """A node to control the mobile MIR Base""" + + config: MIRConfig = MIRConfig() + config_model = MIRConfig + def startup_handler(self): + """MIR startup handler.""" + + self.mir = MiR_Base(mir_ip=self.config.mir_host, map_name=self.config.map_name) + print("MIR Base online") + + def status_handler(self): + """Periodically called to update the current status of the node.""" + """Returns the current state of the UR module""" + if self.node_status.busy == False: + robot_state = self.mir.get_state() + if robot_state == "ERROR": + self.node_status.errored = True + elif robot_state == "EXECUTING": + self.node_status.busy = True + error = "Executing current mission, can still accept more missions." + elif len(self.node_status.running_actions) == 0: + self.node_status.busy = False + + + + @action + def move( + self, + target_location: Annotated[LocationArgument, "Target location name"], + description: Annotated[str, "Description of the location"], + priority: Annotated[Optional[int], "Prority of the movement in the queue. Default is 0."], + ) -> None: + """Sends a move command to the MIR Base""" + self.mir.move( + location_name=target_location.representation, + ) + + + + + + @action + def dock(self, + target_location: Annotated[LocationArgument, "Name of the docking location"], + ) -> None: + """Sends a docking command to the MIR Base""" + self.mir.dock( + location_name=target_location.representation, + ) + self.mir.wait_until_finished() + + @action + def queue_mission( + self, + name: Annotated[List[float], "Name of the mission"], + mission: Annotated[List[dict], "A list of action dictionaries"], + description: Annotated[str, "Description of the mission"], + priority: Annotated[Optional[int], "Prority of the mission in the queue. Defult is 1"], + ) -> None: + """Sends a mission to the MIR Base which could have multiple movement actions""" + self.mir.post_mission_to_queue( + mission_name=name, + act_param_dict=mission, + description=description, + priority=priority, + ) + self.mir.wait_until_finished() + + + @action + def abort_mission_queue( + self + ) -> None: + """Aborts all the missions in the queue""" + self.mir.abort_mission_queue() + + + @action + def add_wait( + self, + delay_seconds: float, + ) -> None: + """Adds a wait mission to MIR Base""" + self.mir.wait(delay_seconds) if __name__ == "__main__": - import uvicorn - - args = parse_args() - - uvicorn.run( - "mirbase_rest_node:app", - host=args.host, - port=args.port, - reload=True, - ws_max_size=100000000000000000000000000000000000000, - ) + MIR_node = MIRNode() + MIR_node.start_node diff --git a/tests/test_module.py b/tests/test_module.py index db0dd40..11fe1e8 100644 --- a/tests/test_module.py +++ b/tests/test_module.py @@ -5,7 +5,8 @@ from pathlib import Path import requests -from wei.core.data_classes import ModuleAbout, WorkcellData +from wei import ExperimentClient +from wei.types import Workcell class TestWEI_Base(unittest.TestCase): @@ -15,47 +16,31 @@ def __init__(self, *args, **kwargs): """Basic setup for WEI's pytest tests""" super().__init__(*args, **kwargs) self.root_dir = Path(__file__).resolve().parent.parent - self.workcell_file = self.root_dir / Path( - "tests/workcell_defs/test_workcell.yaml" - ) - self.workcell = WorkcellData.from_yaml(self.workcell_file) + self.workcell = Workcell.from_yaml(self.root_dir / Path("tests/workcell_defs/test_workcell.yaml")) self.server_host = self.workcell.config.server_host self.server_port = self.workcell.config.server_port self.url = f"http://{self.server_host}:{self.server_port}" - self.module_url = "http://mir_module:3000" + self.module_url = "http://mir_module:3043" self.redis_host = self.workcell.config.redis_host - + self.experiment = ExperimentClient( + server_host=self.server_host, + server_port=self.server_port, + experiment_name="Test_Experiment", + working_dir=Path(__file__).resolve().parent, + ) # Check to see that server is up start_time = time.time() while True: try: - if requests.get(self.url + "/wc/state").status_code == 200: + if requests.get(self.url + "/up").status_code == 200: break except Exception: pass time.sleep(1) if time.time() - start_time > 60: raise TimeoutError("Server did not start in 60 seconds") - while True: - try: - if requests.get(self.module_url + "/state").status_code == 200: - break - except Exception: - pass - time.sleep(1) - if time.time() - start_time > 60: - raise TimeoutError("Module did not start in 60 seconds") - - -class TestModuleInterfaces(TestWEI_Base): - """Tests the basic functionality of the Module.""" - - def test_module_about(self): - """Tests that the module's /about endpoint works""" - response = requests.get(self.module_url + "/about") - assert response.status_code == 200 - ModuleAbout(**response.json()) if __name__ == "__main__": + t = TestWEI_Base() unittest.main() diff --git a/tests/workcell_defs/test_workcell.yaml b/tests/workcell_defs/test_workcell.yaml index c7feb7b..e043768 100644 --- a/tests/workcell_defs/test_workcell.yaml +++ b/tests/workcell_defs/test_workcell.yaml @@ -14,6 +14,6 @@ modules: model: MiR 250 interface: wei_rest_node config: - rest_node_address: "http://mir_module:3000" + rest_node_address: "http://mir_module:3043" workcell_coordinates: [0, 0, 0, 0, 0, 0] locations: {} diff --git a/tests/workflow_defs/test_workflow.yaml b/tests/workflow_defs/test_workflow.yaml index 223d7e0..8b9e90a 100644 --- a/tests/workflow_defs/test_workflow.yaml +++ b/tests/workflow_defs/test_workflow.yaml @@ -12,4 +12,4 @@ flowdef: module: mir_module action: action args: - action_id: 'action1' \ No newline at end of file + action_id: 'action1' diff --git a/wei.compose.yaml b/wei.compose.yaml index c00725f..9d5e757 100644 --- a/wei.compose.yaml +++ b/wei.compose.yaml @@ -11,8 +11,10 @@ services: env_file: .env environment: - PYTHONUNBUFFERED=1 # Fix weird bug with empty logging + - USER_ID=${USER_ID:-1000} + - GROUP_ID=${GROUP_ID:-1000} volumes: - - ${WORKCELLS_DIR}:/workcell_defs + - ./tests/workcell_defs:/workcell_defs - ${WEI_DATA_DIR}:/home/app/.wei - diaspora_config:/home/app/.diaspora command: python3 -m wei.server --workcell /workcell_defs/${WORKCELL_FILENAME} @@ -22,11 +24,13 @@ services: image: ghcr.io/ad-sdl/wei container_name: wei_engine volumes: - - ${WORKCELLS_DIR}:/workcell_defs + - ./tests/workcell_defs:/workcell_defs - ${WEI_DATA_DIR}:/home/app/.wei env_file: .env environment: - - PYTHONUNBUFFERED=1 # Fix weird bug with empty logging + - PYTHONUNBUFFERED=1 # Fix weird bug with empty logging + - USER_ID=${USER_ID:-1000} + - GROUP_ID=${GROUP_ID:-1000} command: python3 -m wei.engine --workcell /workcell_defs/${WORKCELL_FILENAME} depends_on: - wei_redis