From 1404774fb6d4e146079e00989fafeb3464c70745 Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Tue, 13 Aug 2024 19:22:48 -0700 Subject: [PATCH 01/39] Removed the deprecated rest node --- src/mir_rest_node.py | 441 +++++++++++++++++++++++++++++-------------- 1 file changed, 299 insertions(+), 142 deletions(-) diff --git a/src/mir_rest_node.py b/src/mir_rest_node.py index c372820..b745426 100644 --- a/src/mir_rest_node.py +++ b/src/mir_rest_node.py @@ -1,170 +1,327 @@ -#! /usr/bin/env python3 -"""The server for the PF400 robot that takes incoming WEI flow requests from the experiment application""" +"""REST-based node for UR robots""" import datetime -import json -import traceback -from argparse import ArgumentParser, Namespace -from contextlib import asynccontextmanager 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 typing import List + +from fastapi.datastructures import State +from typing_extensions import Annotated +from ur_driver.ur import UR +from wei.modules.rest_module import RESTModule +from wei.types.module_types import ModuleState, ModuleStatus +from wei.types.step_types import ActionRequest, StepResponse, StepStatus +from wei.utils import extract_version + +rest_module = RESTModule( + name="ur_node", + version=extract_version(Path(__file__).parent.parent / "pyproject.toml"), + description="A node to control the ur plate moving robot", + model="ur", +) +rest_module.arg_parser.add_argument( + "--ur_ip", + type=str, + default="164.54.116.129", + help="Hostname or IP address to connect to UR", ) -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() +@rest_module.startup() +def ur_startup(state: State): + """UR startup handler.""" + state.ur = None + state.ur = UR(hostname=state.ur_ip) + print("UR online") -global pf400_ip, pf400_port, state, action_start +@rest_module.state_handler() +def state(state: State): + """Returns the current state of the UR module""" + if state.status not in [ModuleStatus.BUSY, ModuleStatus.ERROR, ModuleStatus.INIT, None] or ( + state.action_start and (datetime.datetime.now() - state.action_start > datetime.timedelta(0, 2)) + ): + # * Gets robot status by checking robot dashboard status messages. + state.ur.ur_dashboard.get_overall_robot_status() + if "NORMAL" not in state.ur.ur_dashboard.safety_status: + state.status = ModuleStatus.ERROR + elif state.ur.get_movement_state() == "BUSY": + state.status = ModuleStatus.BUSY + else: + state.status = ModuleStatus.IDLE + return ModuleState(status=state.status, error="") -@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 +@rest_module.action( + name="gripper_transfer", + description="Execute a transfer in between source and target locations using Robotiq grippers", +) +def gripper_transfer( + state: State, + action: ActionRequest, + home: Annotated[List[float], "Home location"], + source: Annotated[List[float], "Location to transfer sample from"], + target: Annotated[List[float], "Location to transfer sample to"], + source_approach_axis: Annotated[str, "Source location approach axis, (X/Y/Z)"], + target_approach_axis: Annotated[str, "Source location approach axis, (X/Y/Z)"], + source_approach_distance: Annotated[float, "Approach distance in meters"], + target_approach_distance: Annotated[float, "Approach distance in meters"], + gripper_open: Annotated[int, "Set a max value for the gripper open state"], + gripper_close: Annotated[int, "Set a min value for the gripper close state"], +) -> StepResponse: + """Make a transfer using the finger gripper. This function uses linear motions to perform the pick and place movements.""" - Returns - ------- - None""" - global pf400, state, pf400_ip, pf400_port + if not source or not target or not home: # Return Fail + return StepResponse(StepStatus.FAILED, "", "Source, target and home locations must be provided") - args = parse_args() - pf400_ip = args.pf400_ip - pf400_port = args.pf400_port + state.ur.gripper_transfer( + home=home, + source=source, + target=target, + source_approach_distance=source_approach_distance, + target_approach_distance=target_approach_distance, + source_approach_axis=source_approach_axis, + target_approach_axis=target_approach_axis, + gripper_open=gripper_open, + gripper_close=gripper_close, + ) + return StepResponse.step_succeeded(f"Gripper transfer completed from {source} to {target}") - 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 +@rest_module.action( + name="pick_tool", + description="Picks up a tool using the provided tool location", +) +def pick_tool( + state: State, + action: ActionRequest, + home: Annotated[List[float], "Home location"], + tool_loc: Annotated[List[float], "Tool location"], + docking_axis: Annotated[str, "Docking axis, (X/Y/Z)"], + payload: Annotated[float, "Tool payload"], + tool_name: Annotated[str, "Tool name)"], +) -> StepResponse: + """Pick a tool with the UR""" + if not tool_loc or not home: # Return Fail + return StepResponse(StepStatus.FAILED, "", "tool_loc and home locations must be provided") -app = FastAPI( - lifespan=lifespan, + state.ur.pick_tool( + home=home, + tool_loc=tool_loc, + docking_axis=docking_axis, + payload=payload, + tool_name=tool_name, + ) + + return StepResponse.step_succeeded(f"Tool {tool_name} is picked up from {tool_loc}") + + +@rest_module.action( + name="Place_tool", description="Places the attached tool back to the provided tool docking location" ) +def place_tool( + state: State, + action: ActionRequest, + home: Annotated[List[float], "Home location"], + tool_docking: Annotated[List[float], "Tool docking location"], + docking_axis: Annotated[str, "Docking axis, (X/Y/Z)"], + tool_name: Annotated[str, "Tool name)"], +) -> StepResponse: + """Place a tool with the UR""" + state.ur.place_tool( + home=home, + tool_loc=tool_docking, + docking_axis=docking_axis, + tool_name=tool_name, + ) -def check_state(): - """Updates the MiR state + return StepResponse.step_succeeded(f"Tool {tool_name} is placed at {tool_docking}") - Parameters: - ----------- - None - Returns - ------- - None - """ - pass +@rest_module.action( + name="gripper_screw_transfer", + description="Performs a screw transfer using the Robotiq gripper and custom screwdriving bits", +) +def gripper_screw_transfer( + state: State, + action: ActionRequest, + home: Annotated[List[float], "Home location"], + screwdriver_loc: Annotated[List[float], "Screwdriver location"], + screw_loc: Annotated[List[float], "Screw location"], + screw_time: Annotated[int, "Srew time in seconds"], + target: Annotated[List[float], "Location where the srewdriving will be performed"], + gripper_open: Annotated[int, "Set a max value for the gripper open state"], + gripper_close: Annotated[int, "Set a min value for the gripper close state"], +) -> StepResponse: + """Make a screwdriving transfer using Robotiq gripper and custom screwdriving bits with UR""" -@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=[], + if not home or not screwdriver_loc or not screw_loc or not target: + return StepResponse( + StepStatus.FAILED, + "", + "screwdriver_loc, screw_loc and home locations must be provided", + ) + state.ur.gripper_screw_transfer( + home=home, + screwdriver_loc=screwdriver_loc, + screw_loc=screw_loc, + screw_time=screw_time, + target=target, + gripper_open=gripper_open, + gripper_close=gripper_close, ) - 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 + return StepResponse.step_succeeded(f"Screwdriving is completed in between {screw_loc} and {target}") -if __name__ == "__main__": - import uvicorn - args = parse_args() +@rest_module.action( + name="pipette_transfer", + description="Make a pipette transfer to transfer sample liquids in between two locations", +) +def pipette_transfer( + state: State, + action: ActionRequest, + home: Annotated[List[float], "Home location"], + source: Annotated[List[float], "Initial location of the sample"], + target: Annotated[List[float], "Target location of the sample"], + tip_loc=Annotated[List[float], "New tip location"], + tip_trash=Annotated[List[float], "Tip trash location"], + volume=Annotated[float, "Set a volume in micro liters"], +) -> StepResponse: + """Make a pipette transfer for the defined volume with UR""" + + state._state.pipette_transfer( + home=home, + tip_loc=tip_loc, + tip_trash=tip_trash, + source=source, + target=target, + volume=volume, + ) + + return StepResponse.step_succeeded(f"Pipette transfer is completed in between {source} and {target}") - uvicorn.run( - "mirbase_rest_node:app", - host=args.host, - port=args.port, - reload=True, - ws_max_size=100000000000000000000000000000000000000, + +@rest_module.action( + name="pick_and_flip_object", + description="Picks and flips an object 180 degrees", +) +def pick_and_flip_object( + state: State, + action: ActionRequest, + home: Annotated[List[float], "Home location"], + target: Annotated[List[float], "Location of the object"], + approach_axis: Annotated[str, "Approach axis, (X/Y/Z)"], + target_approach_distance: Annotated[float, "Approach distance in meters"], + gripper_open: Annotated[int, "Set a max value for the gripper open state"], + gripper_close: Annotated[int, "Set a min value for the gripper close state"], +) -> StepResponse: + """Picks and flips an object 180 degrees with UR""" + + state.ur.pick_and_flip_object( + home=home, + target=target, + approach_axis=approach_axis, + target_approach_distance=target_approach_distance, + gripper_open=gripper_open, + gripper_close=gripper_close, ) + + return StepResponse.step_succeeded(f"Object is flipped 180 degrees at {target}") + + +@rest_module.action( + name="remove_cap", + description="Removes caps from sample vials", +) +def remove_cap( + state: State, + action: ActionRequest, + home: Annotated[List[float], "Home location"], + source: Annotated[List[float], "Location of the vial cap"], + target: Annotated[ + List[float], + "Location of where the cap will be placed after it is removed from the vail", + ], + gripper_open: Annotated[int, "Set a max value for the gripper open state"], + gripper_close: Annotated[int, "Set a min value for the gripper close state"], +) -> StepResponse: + """Remove caps from sample vials with UR""" + + state.ur.remove_cap( + home=home, + source=source, + target=target, + gripper_open=gripper_open, + gripper_close=gripper_close, + ) + + return StepResponse.step_succeeded(f"Sample vial cap is removed from {source} and placed {target}") + + +@rest_module.action( + name="place_cap", + description="Places caps back to sample vials", +) +def place_cap( + state: State, + action: ActionRequest, + home: Annotated[List[float], "Home location"], + source: Annotated[List[float], "Vail cap initial location"], + target: Annotated[List[float], "The vail location where the cap will installed"], + gripper_open: Annotated[int, "Set a max value for the gripper open state"], + gripper_close: Annotated[int, "Set a min value for the gripper close state"], +) -> StepResponse: + """Places caps back to sample vials with UR""" + + state.ur.place_cap( + home=home, + source=source, + target=target, + gripper_open=gripper_open, + gripper_close=gripper_close, + ) + + return StepResponse.step_succeeded(f"Sample vial cap is placed back to {target}") + + +@rest_module.action( + name="run_urp_program", + description="Runs a URP program on the UR", +) +def run_urp_program( + state: State, + action: ActionRequest, + transfer_file_path=Annotated[str, "Transfer file path"], + program_name=Annotated[str, "Program name"], +) -> StepResponse: + """Run an URP program on the UR""" + + state.ur.run_urp_program( + transfer_file_path=transfer_file_path, + program_name=program_name, + ) + + return StepResponse.step_succeeded(f"URP program {program_name} has been completed") + + +@rest_module.action( + name="set_digital_io", + description="Sets a channel IO output on the UR", +) +def set_digital_io( + state: State, + action: ActionRequest, + channel=Annotated[int, "Channel number"], + value=Annotated[bool, "True/False"], +) -> StepResponse: + """Sets a channel IO output on the UR""" + + state.ur.set_digital_io(channel=channel, value=value) + + return StepResponse.step_succeeded(f"Channel {channel} is set to {value}") + + +if __name__ == "__main__": + rest_module.start() \ No newline at end of file From bfa9ab9c3c9d421c118489639dfc60f7a68e44d3 Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Tue, 13 Aug 2024 21:05:04 -0700 Subject: [PATCH 02/39] Updated rest node --- src/mir_rest_node.py | 326 +++++-------------------------------------- 1 file changed, 37 insertions(+), 289 deletions(-) diff --git a/src/mir_rest_node.py b/src/mir_rest_node.py index b745426..641053e 100644 --- a/src/mir_rest_node.py +++ b/src/mir_rest_node.py @@ -3,325 +3,73 @@ import datetime from pathlib import Path from typing import List - from fastapi.datastructures import State from typing_extensions import Annotated -from ur_driver.ur import UR +from mir_driver.mir_driver import MiR_Base from wei.modules.rest_module import RESTModule from wei.types.module_types import ModuleState, ModuleStatus from wei.types.step_types import ActionRequest, StepResponse, StepStatus from wei.utils import extract_version rest_module = RESTModule( - name="ur_node", + name="mir_node", version=extract_version(Path(__file__).parent.parent / "pyproject.toml"), - description="A node to control the ur plate moving robot", - model="ur", + description="A node to control the mobile MIR Base", + model="Mir250", ) + rest_module.arg_parser.add_argument( - "--ur_ip", + "--mir_host", type=str, - default="164.54.116.129", - help="Hostname or IP address to connect to UR", + default="mirbase2.cels.anl.gov", + help="Hostname or IP address to connect to MIR Base", ) +rest_module.arg_parser.add_argument( + "--mir_key", + type=str, + default="Basic RGlzdHJpYnV0b3I6NjJmMmYwZjFlZmYxMGQzMTUyYzk1ZjZmMDU5NjU3NmU0ODJiYjhlNDQ4MDY0MzNmNGNmOTI5NzkyODM0YjAxNA==", + help="Key for MIR Base", +) @rest_module.startup() -def ur_startup(state: State): - """UR startup handler.""" - state.ur = None - state.ur = UR(hostname=state.ur_ip) - print("UR online") - - +def mir_startup(state: State): + """MIR startup handler.""" + state.mir = None + state.mir = MiR_Base(mir_ip=state.mir_host, mir_key=state.mir_key) + print("MIR Base online") + @rest_module.state_handler() def state(state: State): """Returns the current state of the UR module""" if state.status not in [ModuleStatus.BUSY, ModuleStatus.ERROR, ModuleStatus.INIT, None] or ( state.action_start and (datetime.datetime.now() - state.action_start > datetime.timedelta(0, 2)) ): - # * Gets robot status by checking robot dashboard status messages. - state.ur.ur_dashboard.get_overall_robot_status() - if "NORMAL" not in state.ur.ur_dashboard.safety_status: - state.status = ModuleStatus.ERROR - elif state.ur.get_movement_state() == "BUSY": - state.status = ModuleStatus.BUSY - else: - state.status = ModuleStatus.IDLE + # * Gets robt status + # status = state.mir.status() #TODO: FIX status function to return a status + # if status == "Error": + # state.status = ModuleStatus.ERROR + # elif status == "BUSY": + # state.status = ModuleStatus.BUSY + # else: + state.status = ModuleStatus.IDLE return ModuleState(status=state.status, error="") - @rest_module.action( - name="gripper_transfer", - description="Execute a transfer in between source and target locations using Robotiq grippers", + name="queue_mission", + description="Adds a new mission to the queue. A mission could have multiple movement actions", ) -def gripper_transfer( +def queue_mission( state: State, action: ActionRequest, - home: Annotated[List[float], "Home location"], - source: Annotated[List[float], "Location to transfer sample from"], - target: Annotated[List[float], "Location to transfer sample to"], - source_approach_axis: Annotated[str, "Source location approach axis, (X/Y/Z)"], - target_approach_axis: Annotated[str, "Source location approach axis, (X/Y/Z)"], - source_approach_distance: Annotated[float, "Approach distance in meters"], - target_approach_distance: Annotated[float, "Approach distance in meters"], - gripper_open: Annotated[int, "Set a max value for the gripper open state"], - gripper_close: Annotated[int, "Set a min value for the gripper close state"], + name: Annotated[List[float], "Home location"], + mission: Annotated[List[dict], "Location to transfer sample from"], + description: Annotated[str, "Location to transfer sample to"], + priority: Annotated[int, "Source location approach axis, (X/Y/Z)"], ) -> StepResponse: """Make a transfer using the finger gripper. This function uses linear motions to perform the pick and place movements.""" - - if not source or not target or not home: # Return Fail - return StepResponse(StepStatus.FAILED, "", "Source, target and home locations must be provided") - - state.ur.gripper_transfer( - home=home, - source=source, - target=target, - source_approach_distance=source_approach_distance, - target_approach_distance=target_approach_distance, - source_approach_axis=source_approach_axis, - target_approach_axis=target_approach_axis, - gripper_open=gripper_open, - gripper_close=gripper_close, - ) - return StepResponse.step_succeeded(f"Gripper transfer completed from {source} to {target}") - - -@rest_module.action( - name="pick_tool", - description="Picks up a tool using the provided tool location", -) -def pick_tool( - state: State, - action: ActionRequest, - home: Annotated[List[float], "Home location"], - tool_loc: Annotated[List[float], "Tool location"], - docking_axis: Annotated[str, "Docking axis, (X/Y/Z)"], - payload: Annotated[float, "Tool payload"], - tool_name: Annotated[str, "Tool name)"], -) -> StepResponse: - """Pick a tool with the UR""" - - if not tool_loc or not home: # Return Fail - return StepResponse(StepStatus.FAILED, "", "tool_loc and home locations must be provided") - - state.ur.pick_tool( - home=home, - tool_loc=tool_loc, - docking_axis=docking_axis, - payload=payload, - tool_name=tool_name, - ) - - return StepResponse.step_succeeded(f"Tool {tool_name} is picked up from {tool_loc}") - - -@rest_module.action( - name="Place_tool", description="Places the attached tool back to the provided tool docking location" -) -def place_tool( - state: State, - action: ActionRequest, - home: Annotated[List[float], "Home location"], - tool_docking: Annotated[List[float], "Tool docking location"], - docking_axis: Annotated[str, "Docking axis, (X/Y/Z)"], - tool_name: Annotated[str, "Tool name)"], -) -> StepResponse: - """Place a tool with the UR""" - - state.ur.place_tool( - home=home, - tool_loc=tool_docking, - docking_axis=docking_axis, - tool_name=tool_name, - ) - - return StepResponse.step_succeeded(f"Tool {tool_name} is placed at {tool_docking}") - - -@rest_module.action( - name="gripper_screw_transfer", - description="Performs a screw transfer using the Robotiq gripper and custom screwdriving bits", -) -def gripper_screw_transfer( - state: State, - action: ActionRequest, - home: Annotated[List[float], "Home location"], - screwdriver_loc: Annotated[List[float], "Screwdriver location"], - screw_loc: Annotated[List[float], "Screw location"], - screw_time: Annotated[int, "Srew time in seconds"], - target: Annotated[List[float], "Location where the srewdriving will be performed"], - gripper_open: Annotated[int, "Set a max value for the gripper open state"], - gripper_close: Annotated[int, "Set a min value for the gripper close state"], -) -> StepResponse: - """Make a screwdriving transfer using Robotiq gripper and custom screwdriving bits with UR""" - - if not home or not screwdriver_loc or not screw_loc or not target: - return StepResponse( - StepStatus.FAILED, - "", - "screwdriver_loc, screw_loc and home locations must be provided", - ) - state.ur.gripper_screw_transfer( - home=home, - screwdriver_loc=screwdriver_loc, - screw_loc=screw_loc, - screw_time=screw_time, - target=target, - gripper_open=gripper_open, - gripper_close=gripper_close, - ) - - return StepResponse.step_succeeded(f"Screwdriving is completed in between {screw_loc} and {target}") - - -@rest_module.action( - name="pipette_transfer", - description="Make a pipette transfer to transfer sample liquids in between two locations", -) -def pipette_transfer( - state: State, - action: ActionRequest, - home: Annotated[List[float], "Home location"], - source: Annotated[List[float], "Initial location of the sample"], - target: Annotated[List[float], "Target location of the sample"], - tip_loc=Annotated[List[float], "New tip location"], - tip_trash=Annotated[List[float], "Tip trash location"], - volume=Annotated[float, "Set a volume in micro liters"], -) -> StepResponse: - """Make a pipette transfer for the defined volume with UR""" - - state._state.pipette_transfer( - home=home, - tip_loc=tip_loc, - tip_trash=tip_trash, - source=source, - target=target, - volume=volume, - ) - - return StepResponse.step_succeeded(f"Pipette transfer is completed in between {source} and {target}") - - -@rest_module.action( - name="pick_and_flip_object", - description="Picks and flips an object 180 degrees", -) -def pick_and_flip_object( - state: State, - action: ActionRequest, - home: Annotated[List[float], "Home location"], - target: Annotated[List[float], "Location of the object"], - approach_axis: Annotated[str, "Approach axis, (X/Y/Z)"], - target_approach_distance: Annotated[float, "Approach distance in meters"], - gripper_open: Annotated[int, "Set a max value for the gripper open state"], - gripper_close: Annotated[int, "Set a min value for the gripper close state"], -) -> StepResponse: - """Picks and flips an object 180 degrees with UR""" - - state.ur.pick_and_flip_object( - home=home, - target=target, - approach_axis=approach_axis, - target_approach_distance=target_approach_distance, - gripper_open=gripper_open, - gripper_close=gripper_close, - ) - - return StepResponse.step_succeeded(f"Object is flipped 180 degrees at {target}") - - -@rest_module.action( - name="remove_cap", - description="Removes caps from sample vials", -) -def remove_cap( - state: State, - action: ActionRequest, - home: Annotated[List[float], "Home location"], - source: Annotated[List[float], "Location of the vial cap"], - target: Annotated[ - List[float], - "Location of where the cap will be placed after it is removed from the vail", - ], - gripper_open: Annotated[int, "Set a max value for the gripper open state"], - gripper_close: Annotated[int, "Set a min value for the gripper close state"], -) -> StepResponse: - """Remove caps from sample vials with UR""" - - state.ur.remove_cap( - home=home, - source=source, - target=target, - gripper_open=gripper_open, - gripper_close=gripper_close, - ) - - return StepResponse.step_succeeded(f"Sample vial cap is removed from {source} and placed {target}") - - -@rest_module.action( - name="place_cap", - description="Places caps back to sample vials", -) -def place_cap( - state: State, - action: ActionRequest, - home: Annotated[List[float], "Home location"], - source: Annotated[List[float], "Vail cap initial location"], - target: Annotated[List[float], "The vail location where the cap will installed"], - gripper_open: Annotated[int, "Set a max value for the gripper open state"], - gripper_close: Annotated[int, "Set a min value for the gripper close state"], -) -> StepResponse: - """Places caps back to sample vials with UR""" - - state.ur.place_cap( - home=home, - source=source, - target=target, - gripper_open=gripper_open, - gripper_close=gripper_close, - ) - - return StepResponse.step_succeeded(f"Sample vial cap is placed back to {target}") - - -@rest_module.action( - name="run_urp_program", - description="Runs a URP program on the UR", -) -def run_urp_program( - state: State, - action: ActionRequest, - transfer_file_path=Annotated[str, "Transfer file path"], - program_name=Annotated[str, "Program name"], -) -> StepResponse: - """Run an URP program on the UR""" - - state.ur.run_urp_program( - transfer_file_path=transfer_file_path, - program_name=program_name, - ) - - return StepResponse.step_succeeded(f"URP program {program_name} has been completed") - - -@rest_module.action( - name="set_digital_io", - description="Sets a channel IO output on the UR", -) -def set_digital_io( - state: State, - action: ActionRequest, - channel=Annotated[int, "Channel number"], - value=Annotated[bool, "True/False"], -) -> StepResponse: - """Sets a channel IO output on the UR""" - - state.ur.set_digital_io(channel=channel, value=value) - - return StepResponse.step_succeeded(f"Channel {channel} is set to {value}") - + state.post_mission_to_queue(mission_name=name, act_param_dict=mission, description = description, priority = priority) + return StepResponse.step_succeeded(f"Mission {name} is sent to MIR Base") if __name__ == "__main__": rest_module.start() \ No newline at end of file From 3385d84405baa87182182cc3b82afa5738a5c40e Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Tue, 13 Aug 2024 21:08:29 -0700 Subject: [PATCH 03/39] Dockstring update --- src/mir_rest_node.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/mir_rest_node.py b/src/mir_rest_node.py index 641053e..45ff29d 100644 --- a/src/mir_rest_node.py +++ b/src/mir_rest_node.py @@ -2,7 +2,7 @@ import datetime from pathlib import Path -from typing import List +from typing import List, Optional from fastapi.datastructures import State from typing_extensions import Annotated from mir_driver.mir_driver import MiR_Base @@ -62,10 +62,10 @@ def state(state: State): def queue_mission( state: State, action: ActionRequest, - name: Annotated[List[float], "Home location"], - mission: Annotated[List[dict], "Location to transfer sample from"], - description: Annotated[str, "Location to transfer sample to"], - priority: Annotated[int, "Source location approach axis, (X/Y/Z)"], + 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"], ) -> StepResponse: """Make a transfer using the finger gripper. This function uses linear motions to perform the pick and place movements.""" state.post_mission_to_queue(mission_name=name, act_param_dict=mission, description = description, priority = priority) From e75c545257f1820999df485479e8fca5d1a25e28 Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Wed, 14 Aug 2024 13:07:05 -0700 Subject: [PATCH 04/39] Editing docstringg --- src/mir_rest_node.py | 36 +++++++++++++++++++++++++++--------- 1 file changed, 27 insertions(+), 9 deletions(-) diff --git a/src/mir_rest_node.py b/src/mir_rest_node.py index 45ff29d..9f1c5cb 100644 --- a/src/mir_rest_node.py +++ b/src/mir_rest_node.py @@ -3,12 +3,13 @@ import datetime from pathlib import Path from typing import List, Optional + from fastapi.datastructures import State -from typing_extensions import Annotated from mir_driver.mir_driver import MiR_Base +from typing_extensions import Annotated from wei.modules.rest_module import RESTModule from wei.types.module_types import ModuleState, ModuleStatus -from wei.types.step_types import ActionRequest, StepResponse, StepStatus +from wei.types.step_types import ActionRequest, StepResponse from wei.utils import extract_version rest_module = RESTModule( @@ -32,18 +33,26 @@ help="Key for MIR Base", ) + @rest_module.startup() def mir_startup(state: State): """MIR startup handler.""" state.mir = None state.mir = MiR_Base(mir_ip=state.mir_host, mir_key=state.mir_key) print("MIR Base online") - + + @rest_module.state_handler() def state(state: State): """Returns the current state of the UR module""" - if state.status not in [ModuleStatus.BUSY, ModuleStatus.ERROR, ModuleStatus.INIT, None] or ( - state.action_start and (datetime.datetime.now() - state.action_start > datetime.timedelta(0, 2)) + if state.status not in [ + ModuleStatus.BUSY, + ModuleStatus.ERROR, + ModuleStatus.INIT, + None, + ] or ( + state.action_start + and (datetime.datetime.now() - state.action_start > datetime.timedelta(0, 2)) ): # * Gets robt status # status = state.mir.status() #TODO: FIX status function to return a status @@ -55,6 +64,7 @@ def state(state: State): state.status = ModuleStatus.IDLE return ModuleState(status=state.status, error="") + @rest_module.action( name="queue_mission", description="Adds a new mission to the queue. A mission could have multiple movement actions", @@ -65,11 +75,19 @@ def queue_mission( 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"], + priority: Annotated[ + Optional[int], "Prority of the mission in the queue. Defult is 1" + ], ) -> StepResponse: - """Make a transfer using the finger gripper. This function uses linear motions to perform the pick and place movements.""" - state.post_mission_to_queue(mission_name=name, act_param_dict=mission, description = description, priority = priority) + """Sends a mission to the MIR Base which could have multiple movement actions""" + state.post_mission_to_queue( + mission_name=name, + act_param_dict=mission, + description=description, + priority=priority, + ) return StepResponse.step_succeeded(f"Mission {name} is sent to MIR Base") + if __name__ == "__main__": - rest_module.start() \ No newline at end of file + rest_module.start() From 5427b309920e57f9928d801556b0e84592a9615f Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Wed, 14 Aug 2024 13:10:02 -0700 Subject: [PATCH 05/39] Update run command --- compose.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/compose.yaml b/compose.yaml index bd7ad2f..e71b6e6 100644 --- a/compose.yaml +++ b/compose.yaml @@ -10,7 +10,7 @@ services: - ${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 ${MIR_PORT} --mir_url ${MIR_URL} --mir_key ${MIR_KEY} env_file: .env volumes: - ./src:/home/app/mir_module/src From c93d8346e97e83f1b3059c9d87ab9c1a96af291c Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Wed, 14 Aug 2024 13:10:19 -0700 Subject: [PATCH 06/39] Update args --- example.env | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/example.env b/example.env index 31d2bfc..0ef703f 100644 --- a/example.env +++ b/example.env @@ -1,9 +1,7 @@ # 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_PORT=3043 +MIR_URL="mirbase2.cels.anl.gov" +MIR_KEY="Basic RGlzdHJpYnV0b3I6NjJmMmYwZjFlZmYxMGQzMTUyYzk1ZjZmMDU5NjU3NmU0ODJiYjhlNDQ4MDY0MzNmNGNmOTI5NzkyODM0YjAxNA==" WEI_DATA_DIR=~/.wei WORKCELL_FILENAME=test_workcell.yaml WORKCELLS_DIR=${PROJECT_PATH}/tests/workcell_defs From dedfa5669d570b41309fa4668f7ae8883faf3048 Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Wed, 14 Aug 2024 17:39:22 -0700 Subject: [PATCH 07/39] Adding issue templates --- .github/ISSUE_TEMPLATE/bug_report.md | 30 +++++++++++++++++++ .github/ISSUE_TEMPLATE/feature_request.md | 20 +++++++++++++ .github/ISSUE_TEMPLATE/other.md | 10 +++++++ .../ISSUE_TEMPLATE/question-issue-template.md | 10 +++++++ 4 files changed, 70 insertions(+) create mode 100644 .github/ISSUE_TEMPLATE/bug_report.md create mode 100644 .github/ISSUE_TEMPLATE/feature_request.md create mode 100644 .github/ISSUE_TEMPLATE/other.md create mode 100644 .github/ISSUE_TEMPLATE/question-issue-template.md 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** From fd3ec7de151811ea88878d4372f3abbbad6bca96 Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Wed, 14 Aug 2024 17:39:34 -0700 Subject: [PATCH 08/39] Removed --- .github/workflows/docker-build.yml | 76 ------------------------------ 1 file changed, 76 deletions(-) delete mode 100644 .github/workflows/docker-build.yml 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 From 002b047417ac233b62f402b1b2a57dd9301988d0 Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Wed, 14 Aug 2024 17:39:48 -0700 Subject: [PATCH 09/39] Update --- .github/workflows/docker-publish.yml | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) 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 From 29fa7038ab909b861fdf9466832fb3b7f647ca68 Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Wed, 14 Aug 2024 17:40:06 -0700 Subject: [PATCH 10/39] Updates --- Makefile | 19 +++---------------- 1 file changed, 3 insertions(+), 16 deletions(-) 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 From 24e7444e5835a419dccc2eae2317bd638b56a4af Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Wed, 14 Aug 2024 17:40:29 -0700 Subject: [PATCH 11/39] Updated port --- tests/workcell_defs/test_workcell.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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: {} From 56e9ba153dc67b13e00a94cee3fd3509c83f6902 Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Wed, 14 Aug 2024 17:40:50 -0700 Subject: [PATCH 12/39] Updated port --- example.env | 5 ++--- tests/test_module.py | 2 +- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/example.env b/example.env index 0ef703f..716bf7f 100644 --- a/example.env +++ b/example.env @@ -1,7 +1,6 @@ # Note: all paths are relative to the docker compose file -MIR_PORT=3043 -MIR_URL="mirbase2.cels.anl.gov" -MIR_KEY="Basic RGlzdHJpYnV0b3I6NjJmMmYwZjFlZmYxMGQzMTUyYzk1ZjZmMDU5NjU3NmU0ODJiYjhlNDQ4MDY0MzNmNGNmOTI5NzkyODM0YjAxNA==" +MIR_HOST="mirbase2.cels.anl.gov" +MIR_KEY="BasicRGlzdHJpYnV0b3I6NjJmMmYwZjFlZmYxMGQzMTUyYzk1ZjZmMDU5NjU3NmU0ODJiYjhlNDQ4MDY0MzNmNGNmOTI5NzkyODM0YjAxNA==" WEI_DATA_DIR=~/.wei WORKCELL_FILENAME=test_workcell.yaml WORKCELLS_DIR=${PROJECT_PATH}/tests/workcell_defs diff --git a/tests/test_module.py b/tests/test_module.py index db0dd40..01530ca 100644 --- a/tests/test_module.py +++ b/tests/test_module.py @@ -22,7 +22,7 @@ def __init__(self, *args, **kwargs): 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 # Check to see that server is up From 228cfeb3ee8c72aa9b554eaa0d52cf04ee3c128b Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Wed, 14 Aug 2024 17:41:00 -0700 Subject: [PATCH 13/39] Updated port --- compose.yaml | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/compose.yaml b/compose.yaml index e71b6e6..ef68e34 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 ${MIR_PORT} --mir_url ${MIR_URL} --mir_key ${MIR_KEY} + command: python -m mir_rest_node --port 3043 --mir_host ${MIR_HOST} --mir_key ${MIR_KEY} env_file: .env volumes: - ./src:/home/app/mir_module/src - ./tests:/home/app/mir_module/tests ports: - - 3000:3000 + - 3043:3043 From da63a328d95f128f290634d1ee2b7eb5117a06fb Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Wed, 14 Aug 2024 17:41:17 -0700 Subject: [PATCH 14/39] Clean up --- pyproject.toml | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 4156ea0..0068686 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -5,6 +5,10 @@ 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"}, ] dependencies = [ "ad_sdl.wei>=0.5.3", @@ -60,7 +64,7 @@ exclude = [ ] # Same as Black. -line-length = 88 +line-length = 120 indent-width = 4 # Assume Python 3.8 From 70d105105398900f4013530e850bdfad0dc9a080 Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Wed, 14 Aug 2024 17:41:24 -0700 Subject: [PATCH 15/39] Clean up --- wei.compose.yaml | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) 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 From 4fb385ede7948da07f43a1a6d1a1b9ecee8c4098 Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Wed, 14 Aug 2024 17:43:02 -0700 Subject: [PATCH 16/39] Bug fix --- example.env | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/example.env b/example.env index 716bf7f..0a6fe33 100644 --- a/example.env +++ b/example.env @@ -3,6 +3,6 @@ MIR_HOST="mirbase2.cels.anl.gov" MIR_KEY="BasicRGlzdHJpYnV0b3I6NjJmMmYwZjFlZmYxMGQzMTUyYzk1ZjZmMDU5NjU3NmU0ODJiYjhlNDQ4MDY0MzNmNGNmOTI5NzkyODM0YjAxNA==" 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 From 8e4d450011bb8dfac01263e5eb13b8f913f9188b Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Thu, 15 Aug 2024 13:50:09 -0700 Subject: [PATCH 17/39] Removed MIR Key argument --- compose.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/compose.yaml b/compose.yaml index ef68e34..a0c7e61 100644 --- a/compose.yaml +++ b/compose.yaml @@ -9,7 +9,7 @@ services: tags: - ${IMAGE}:latest - ${IMAGE}:dev - command: python -m mir_rest_node --port 3043 --mir_host ${MIR_HOST} --mir_key ${MIR_KEY} + command: python -m mir_rest_node --port 3043 --mir_host ${MIR_HOST} env_file: .env volumes: - ./src:/home/app/mir_module/src From c8d4bf8b0d43d92420784854efee073ab109b7a8 Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Thu, 15 Aug 2024 13:50:19 -0700 Subject: [PATCH 18/39] Removed MIR Key argument --- example.env | 1 - 1 file changed, 1 deletion(-) diff --git a/example.env b/example.env index 0a6fe33..7466d07 100644 --- a/example.env +++ b/example.env @@ -1,6 +1,5 @@ # Note: all paths are relative to the docker compose file MIR_HOST="mirbase2.cels.anl.gov" -MIR_KEY="BasicRGlzdHJpYnV0b3I6NjJmMmYwZjFlZmYxMGQzMTUyYzk1ZjZmMDU5NjU3NmU0ODJiYjhlNDQ4MDY0MzNmNGNmOTI5NzkyODM0YjAxNA==" WEI_DATA_DIR=~/.wei WORKCELL_FILENAME=test_workcell.yaml WORKCELLS_DIR=./tests/workcell_defs From ed5a9c97229181b53b68af30ae5afcd711f503ed Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Thu, 15 Aug 2024 13:50:25 -0700 Subject: [PATCH 19/39] Removed MIR Key argument --- src/mir_rest_node.py | 16 ++-------------- 1 file changed, 2 insertions(+), 14 deletions(-) diff --git a/src/mir_rest_node.py b/src/mir_rest_node.py index 9f1c5cb..4748b5a 100644 --- a/src/mir_rest_node.py +++ b/src/mir_rest_node.py @@ -26,13 +26,6 @@ help="Hostname or IP address to connect to MIR Base", ) -rest_module.arg_parser.add_argument( - "--mir_key", - type=str, - default="Basic RGlzdHJpYnV0b3I6NjJmMmYwZjFlZmYxMGQzMTUyYzk1ZjZmMDU5NjU3NmU0ODJiYjhlNDQ4MDY0MzNmNGNmOTI5NzkyODM0YjAxNA==", - help="Key for MIR Base", -) - @rest_module.startup() def mir_startup(state: State): @@ -50,10 +43,7 @@ def state(state: State): ModuleStatus.ERROR, ModuleStatus.INIT, None, - ] or ( - state.action_start - and (datetime.datetime.now() - state.action_start > datetime.timedelta(0, 2)) - ): + ] or (state.action_start and (datetime.datetime.now() - state.action_start > datetime.timedelta(0, 2))): # * Gets robt status # status = state.mir.status() #TODO: FIX status function to return a status # if status == "Error": @@ -75,9 +65,7 @@ def queue_mission( 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" - ], + priority: Annotated[Optional[int], "Prority of the mission in the queue. Defult is 1"], ) -> StepResponse: """Sends a mission to the MIR Base which could have multiple movement actions""" state.post_mission_to_queue( From ee8b2413c9ee46cd4717b031febe9bcb49f53bfb Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Thu, 15 Aug 2024 14:40:44 -0700 Subject: [PATCH 20/39] Added Move and Dock actions --- src/mir_rest_node.py | 40 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 40 insertions(+) diff --git a/src/mir_rest_node.py b/src/mir_rest_node.py index 4748b5a..b6dfaf7 100644 --- a/src/mir_rest_node.py +++ b/src/mir_rest_node.py @@ -55,6 +55,46 @@ def state(state: State): return ModuleState(status=state.status, error="") +@rest_module.action( + name="move", + description="Send a Move command to the MIR Base", +) +def move( + state: State, + action: ActionRequest, + target_location: Annotated[List[dict], "Target location name"], + description: Annotated[str, "Description of the location"], + priority: Annotated[Optional[int], "Prority of the movement in the queue. Defult is 1"], +) -> StepResponse: + """Sends a move command to the MIR Base""" + state.move( + location_name=target_location, + description=description, + priority=priority, + ) + return StepResponse.step_succeeded(f"MIR Base moved to the location: {target_location} ") + + +@rest_module.action( + name="dock", + description="Sends a dock command to the MIR Base", +) +def dock( + state: State, + action: ActionRequest, + target_location: Annotated[List[dict], "Name of the docking location"], + description: Annotated[str, "Description of the docking location"], + priority: Annotated[Optional[int], "Prority of the docking in the queue. Defult is 1"], +) -> StepResponse: + """Sends a docking command to the MIR Base""" + state.dock( + location_name=target_location, + description=description, + priority=priority, + ) + return StepResponse.step_succeeded(f"MIR Base moved to the location: {target_location} ") + + @rest_module.action( name="queue_mission", description="Adds a new mission to the queue. A mission could have multiple movement actions", From 4daaa46203f236f4cc642cd0a37ad4d4a48c42f3 Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Thu, 15 Aug 2024 14:48:55 -0700 Subject: [PATCH 21/39] Added abort mission --- src/mir_rest_node.py | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/src/mir_rest_node.py b/src/mir_rest_node.py index b6dfaf7..2f99511 100644 --- a/src/mir_rest_node.py +++ b/src/mir_rest_node.py @@ -69,8 +69,6 @@ def move( """Sends a move command to the MIR Base""" state.move( location_name=target_location, - description=description, - priority=priority, ) return StepResponse.step_succeeded(f"MIR Base moved to the location: {target_location} ") @@ -83,14 +81,10 @@ def dock( state: State, action: ActionRequest, target_location: Annotated[List[dict], "Name of the docking location"], - description: Annotated[str, "Description of the docking location"], - priority: Annotated[Optional[int], "Prority of the docking in the queue. Defult is 1"], ) -> StepResponse: """Sends a docking command to the MIR Base""" state.dock( location_name=target_location, - description=description, - priority=priority, ) return StepResponse.step_succeeded(f"MIR Base moved to the location: {target_location} ") @@ -117,5 +111,18 @@ def queue_mission( return StepResponse.step_succeeded(f"Mission {name} is sent to MIR Base") +@rest_module.action( + name="abort_mission_queue", + description="Send a abort_mission_queue command to the MIR Base", +) +def abort_mission_queue( + state: State, + action: ActionRequest, +) -> StepResponse: + """Aborts all the missions in the queue""" + state.abort_mission_queue() + return StepResponse.step_succeeded("Missions aborted") + + if __name__ == "__main__": rest_module.start() From b9ed13aa71bd12b00b0abdd75a541d1d37709333 Mon Sep 17 00:00:00 2001 From: AileenCleary Date: Tue, 20 Aug 2024 10:31:44 -0500 Subject: [PATCH 22/39] Precommit.. --- src/mir_driver/locations.json | 46 ++ src/mir_driver/mir_driver.py | 952 +++++++++++++++++----------------- 2 files changed, 532 insertions(+), 466 deletions(-) create mode 100644 src/mir_driver/locations.json diff --git a/src/mir_driver/locations.json b/src/mir_driver/locations.json new file mode 100644 index 0000000..1803429 --- /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": 9.65, + "guid": "88c7d8ff-54d0-11ef-90e0-0001297b4d50", + "orientation": 180.0, + "pos_y": 20.25, + "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..8ab03d0 100644 --- a/src/mir_driver/mir_driver.py +++ b/src/mir_driver/mir_driver.py @@ -1,15 +1,14 @@ #!/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 from pprint import pprint +import requests + class MiR_Base: """Main Driver Class for the MiR Robotic base.""" @@ -23,21 +22,21 @@ def __init__( action_dict=None, position_dict=None, curr_mission_queue_id=None, - filename=None + filename="src/mir_driver/locations.json", ): """ - 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() @@ -47,644 +46,675 @@ def __init__( self.position_dict = self.create_position_dict() self.curr_mission_queue_id = self.set_mission_queue_id() - 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. + + 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. - Missions = {"description" : description,"group_id" : self.group_id,"name" : mission_name} + Returns: + dict: The response from the MiR base after initializing the mission. + """ - response = self.send_command("missions", Missions, printq, "New mission successfully added") + 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} + if param["id"] == k or param["input_name"] == k: + param["value"] = v - url = "missions/" + mission_id + "/actions/" + action_id - change_action = self.change_command(url, PutMission_action, printq, "Action successfully changed.") - - return + 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=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}]} + """ + 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_queue_payload = {"mission_id": mission_id, "priority": priority} + response = self.send_command( + "mission_queue", mission_queue_payload, printq, "Mission successfully added to queue." + ) - Mission_queues = {"mission_id" : mission_id,"priority" : priority} - - response = self.send_command("mission_queue", Mission_queues, printq, "Mission successfully added to queue.") - return response def check_queue_completion(self): + """ + Check and print the status of the current mission queue and its actions. - mission_queue = self.get_mission_queue() - width = 50 - - 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 - - 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") + self.find_mission_in_queue(mission_name) return def status(self): - ''' - status : Retrieves MiR system status. - ''' - self_status = self.receive_response("status", True) + """ + 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. + + 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 == 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 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' - }, - { - 'id' : 'cart_entry_position', - 'input_name' : None, - 'name' : 'Main', - 'value' : 'main' - }, + "move": { + "parameters": [ { - 'id' : 'main_or_entry_position', - 'input_name' : None, - 'name' : 'Main', - 'value' : 'main' + "id": "position", + "input_name": None, + "name": "another_move", + "value": "b34d6e54-5670-11ef-a572-0001297b4d50", }, - { - '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" : [ - { - 'id' : 'marker', - 'input_name' : None, - 'name' : 'camera_marker', - 'value' : '4ccacd0d-7f46-11ee-8521-0001297b4d50' - }, + "docking": { + "parameters": [ { - 'id' : 'marker_type', - 'input_name' : None, - 'name' : 'Narrow asymmetric MiR500/1000 shelf', - 'value' : 'mirconst-guid-0000-0001-marker000001' + "id": "marker", + "input_name": None, + "name": "camera_marker", + "value": "4ccacd0d-7f46-11ee-8521-0001297b4d50", }, { - 'id' : 'retries', - 'input_name' : None, - 'value' : 10 + "id": "marker_type", + "input_name": None, + "name": "Narrow asymmetric MiR500/1000 shelf", + "value": "mirconst-guid-0000-0001-marker000001", }, - { - '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.1}, + ] + }, } 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) + def move(self, location): + """ + 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}_{dt.datetime.now()}" + with open(self.filename) as f: + data = json.load(f) + guid = data[self.map_name][location]["guid"] - move =self.post_mission_to_queue(mission_name,[{"move": {"position":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) + def dock(self, location): + """ + 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}_{dt.datetime.now()}" + with open(self.filename) as f: + data = json.load(f) + guid = data[self.map_name][location]["guid"] - dock =self.post_mission_to_queue(mission_name,[{"docking": {"marker":guid}}], printq=True) + dock = self.post_mission_to_queue(mission_name, [{"docking": {"marker": guid}}], printq=True) return dock if __name__ == "__main__": - mir_base = MiR_Base(map_name="RPL", filename="locations.json") + mir_base = MiR_Base(map_name="RPL") # response = requests.post( # mir_base.host + "mission_queue/search", @@ -707,9 +737,9 @@ 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: @@ -720,30 +750,20 @@ def dock(self, location, mission_name): # 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") + # time.sleep(20) + # mir_base.check_queue_completion() From e505410a3c2b7aa08320cff07843b599c2125bab Mon Sep 17 00:00:00 2001 From: AileenCleary Date: Tue, 20 Aug 2024 10:42:54 -0500 Subject: [PATCH 23/39] pre-commit.. --- README.md | 2 +- tests/test_module.py | 4 +--- tests/workflow_defs/test_workflow.yaml | 2 +- 3 files changed, 3 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index f9f646c..9dba7bb 100644 --- a/README.md +++ b/README.md @@ -1 +1 @@ -# mir_module \ No newline at end of file +# mir_module diff --git a/tests/test_module.py b/tests/test_module.py index 01530ca..ca0fd65 100644 --- a/tests/test_module.py +++ b/tests/test_module.py @@ -15,9 +15,7 @@ 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_file = self.root_dir / Path("tests/workcell_defs/test_workcell.yaml") self.workcell = WorkcellData.from_yaml(self.workcell_file) self.server_host = self.workcell.config.server_host self.server_port = self.workcell.config.server_port 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' From a76cc08d518ad13a89f84d712846ddc405b0fe83 Mon Sep 17 00:00:00 2001 From: AileenCleary Date: Tue, 20 Aug 2024 11:21:39 -0500 Subject: [PATCH 24/39] Added wait functionality for experiments. --- src/mir_driver/locations.json | 6 +++--- src/mir_driver/mir_driver.py | 27 ++++++++++++++++++++++++--- 2 files changed, 27 insertions(+), 6 deletions(-) diff --git a/src/mir_driver/locations.json b/src/mir_driver/locations.json index 1803429..ac05e2f 100644 --- a/src/mir_driver/locations.json +++ b/src/mir_driver/locations.json @@ -22,10 +22,10 @@ "type_id": 11 }, "test_pos": { - "pos_x": 9.65, + "pos_x": 10.119, "guid": "88c7d8ff-54d0-11ef-90e0-0001297b4d50", - "orientation": 180.0, - "pos_y": 20.25, + "orientation": 88.936, + "pos_y": 19.0, "type_id": 0 }, "test_move": { diff --git a/src/mir_driver/mir_driver.py b/src/mir_driver/mir_driver.py index 8ab03d0..9b2f78f 100644 --- a/src/mir_driver/mir_driver.py +++ b/src/mir_driver/mir_driver.py @@ -331,7 +331,7 @@ def set_action_params(self, mission_id, act_param_dict, printq): return - def post_mission_to_queue(self, mission_name, act_param_dict, description="", priority=1, printq=False): + 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. @@ -607,6 +607,7 @@ def create_action_dict(self): {"id": "max_linear_speed", "input_name": None, "value": 0.1}, ] }, + "wait": {"parameters": [{"id": "time", "input_name": None, "value": "00:00:05.000000"}]}, } return action_dict @@ -708,10 +709,25 @@ def dock(self, location): data = json.load(f) guid = data[self.map_name][location]["guid"] - dock = self.post_mission_to_queue(mission_name, [{"docking": {"marker": guid}}], printq=True) + dock = self.post_mission_to_queue(mission_name, [{"docking": {"marker": guid}}]) return dock + def wait(self, time): + """ + Creates a mission to wait for a specified time and adds it to the mission queue. + + Args: + time (str): The amount of time to wait for. + + Returns: + dict: The response from posting the mission to the queue. + """ + 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") @@ -744,7 +760,7 @@ def dock(self, location): ## 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) @@ -767,3 +783,8 @@ def dock(self, location): # mir_base.move("another_move") # time.sleep(20) # mir_base.check_queue_completion() + mir_base.abort_mission_queue() + for _ in range(15): + mir_base.move("test_pos") + mir_base.dock("charger1") + mir_base.wait("00:01:00.000000") From 866cc22dc5c3b1a255777b45a8464943c10fbfe9 Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Tue, 20 Aug 2024 12:55:38 -0700 Subject: [PATCH 25/39] Added wait action --- src/mir_rest_node.py | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/src/mir_rest_node.py b/src/mir_rest_node.py index 2f99511..dcbb707 100644 --- a/src/mir_rest_node.py +++ b/src/mir_rest_node.py @@ -44,6 +44,7 @@ def state(state: State): ModuleStatus.INIT, None, ] or (state.action_start and (datetime.datetime.now() - state.action_start > datetime.timedelta(0, 2))): + robot_state = state.mir.get_state() # * Gets robt status # status = state.mir.status() #TODO: FIX status function to return a status # if status == "Error": @@ -63,12 +64,10 @@ def move( state: State, action: ActionRequest, target_location: Annotated[List[dict], "Target location name"], - description: Annotated[str, "Description of the location"], - priority: Annotated[Optional[int], "Prority of the movement in the queue. Defult is 1"], ) -> StepResponse: """Sends a move command to the MIR Base""" state.move( - location_name=target_location, + location=target_location, ) return StepResponse.step_succeeded(f"MIR Base moved to the location: {target_location} ") @@ -84,7 +83,7 @@ def dock( ) -> StepResponse: """Sends a docking command to the MIR Base""" state.dock( - location_name=target_location, + location=target_location, ) return StepResponse.step_succeeded(f"MIR Base moved to the location: {target_location} ") @@ -123,6 +122,18 @@ def abort_mission_queue( state.abort_mission_queue() return StepResponse.step_succeeded("Missions aborted") +@rest_module.action( + name="add_wait", + description="Send a abort_mission_queue command to the MIR Base", +) +def add_wait( + state: State, + action: ActionRequest, + delay_seconds: float, +) -> StepResponse: + """Adds a wait mission to MIR Base""" + state.wait(delay_seconds) + return StepResponse.step_succeeded("Missions aborted") if __name__ == "__main__": rest_module.start() From 57cb47ec5eb6a4cd5bf65ad3b215e3d484d7ecfe Mon Sep 17 00:00:00 2001 From: AileenCleary Date: Tue, 20 Aug 2024 14:57:37 -0500 Subject: [PATCH 26/39] Converted seconds to datetime, accept seconds as arg now. --- src/mir_driver/mir_driver.py | 17 ++++++++++++----- src/mir_rest_node.py | 2 +- 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/src/mir_driver/mir_driver.py b/src/mir_driver/mir_driver.py index 9b2f78f..f285745 100644 --- a/src/mir_driver/mir_driver.py +++ b/src/mir_driver/mir_driver.py @@ -5,6 +5,7 @@ import datetime as dt import json +import time from pprint import pprint import requests @@ -365,7 +366,7 @@ def post_mission_to_queue(self, mission_name, act_param_dict, description="", pr return response - def check_queue_completion(self): + def check_queue_completion(self, printq=False): """ Check and print the status of the current mission queue and its actions. @@ -396,7 +397,8 @@ def check_queue_completion(self): print(f"\r[{bar}] {percent:6.2f}% Queue Complete\n") print(f"Current Mission: {mission_name}\n") - self.find_mission_in_queue(mission_name) + if printq: + self.find_mission_in_queue(mission_name) return @@ -604,7 +606,7 @@ def create_action_dict(self): "value": "mirconst-guid-0000-0001-marker000001", }, {"id": "retries", "input_name": None, "value": 10}, - {"id": "max_linear_speed", "input_name": None, "value": 0.1}, + {"id": "max_linear_speed", "input_name": None, "value": 0.3}, ] }, "wait": {"parameters": [{"id": "time", "input_name": None, "value": "00:00:05.000000"}]}, @@ -723,6 +725,7 @@ def wait(self, time): Returns: dict: The response from posting the mission to the queue. """ + time = str(dt.timedelta(seconds=time)) mission_name = f"wait_for_{time}_{dt.datetime.now()}" wait = self.post_mission_to_queue(mission_name, [{"wait": {"time": time}}]) @@ -784,7 +787,11 @@ def wait(self, time): # time.sleep(20) # mir_base.check_queue_completion() mir_base.abort_mission_queue() - for _ in range(15): + for _ in range(30): mir_base.move("test_pos") mir_base.dock("charger1") - mir_base.wait("00:01:00.000000") + 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 2f99511..edf4b67 100644 --- a/src/mir_rest_node.py +++ b/src/mir_rest_node.py @@ -64,7 +64,7 @@ def move( action: ActionRequest, target_location: Annotated[List[dict], "Target location name"], description: Annotated[str, "Description of the location"], - priority: Annotated[Optional[int], "Prority of the movement in the queue. Defult is 1"], + priority: Annotated[Optional[int], "Prority of the movement in the queue. Default is 1"], ) -> StepResponse: """Sends a move command to the MIR Base""" state.move( From 76bd0a600f1b59a60f13fd4a768fe8cd4ae3b9df Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Tue, 20 Aug 2024 20:52:22 -0700 Subject: [PATCH 27/39] Added robot state checks --- src/mir_rest_node.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/mir_rest_node.py b/src/mir_rest_node.py index dcbb707..36eb3aa 100644 --- a/src/mir_rest_node.py +++ b/src/mir_rest_node.py @@ -45,14 +45,12 @@ def state(state: State): None, ] or (state.action_start and (datetime.datetime.now() - state.action_start > datetime.timedelta(0, 2))): robot_state = state.mir.get_state() - # * Gets robt status - # status = state.mir.status() #TODO: FIX status function to return a status - # if status == "Error": - # state.status = ModuleStatus.ERROR - # elif status == "BUSY": - # state.status = ModuleStatus.BUSY - # else: - state.status = ModuleStatus.IDLE + if robot_state == "ERROR": + state.status = ModuleStatus.ERROR + elif robot_state == "BUSY": + state.status = ModuleStatus.BUSY + else: + state.status = ModuleStatus.IDLE return ModuleState(status=state.status, error="") @@ -122,6 +120,7 @@ def abort_mission_queue( state.abort_mission_queue() return StepResponse.step_succeeded("Missions aborted") + @rest_module.action( name="add_wait", description="Send a abort_mission_queue command to the MIR Base", @@ -135,5 +134,6 @@ def add_wait( state.wait(delay_seconds) return StepResponse.step_succeeded("Missions aborted") + if __name__ == "__main__": rest_module.start() From 4782ebb86406b7813b714a5246c1cb5b333d463b Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Tue, 20 Aug 2024 20:52:32 -0700 Subject: [PATCH 28/39] Fixing import errors --- tests/test_module.py | 35 +++++++++++------------------------ 1 file changed, 11 insertions(+), 24 deletions(-) diff --git a/tests/test_module.py b/tests/test_module.py index ca0fd65..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,45 +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: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() From 053872501df110438fa207a2da62579449c4c395 Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Tue, 20 Aug 2024 20:55:36 -0700 Subject: [PATCH 29/39] Removed key argument --- src/mir_rest_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mir_rest_node.py b/src/mir_rest_node.py index 0ead050..eac2022 100644 --- a/src/mir_rest_node.py +++ b/src/mir_rest_node.py @@ -31,7 +31,7 @@ def mir_startup(state: State): """MIR startup handler.""" state.mir = None - state.mir = MiR_Base(mir_ip=state.mir_host, mir_key=state.mir_key) + state.mir = MiR_Base(mir_ip=state.mir_host) print("MIR Base online") From 8c58097cdd55d1e8e6caaae7a5c2b23fc10b6b96 Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Tue, 20 Aug 2024 23:19:17 -0500 Subject: [PATCH 30/39] Added map name argument --- compose.yaml | 2 +- example.env | 1 + src/mir_rest_node.py | 9 +++++++-- 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/compose.yaml b/compose.yaml index a0c7e61..f64e2c0 100644 --- a/compose.yaml +++ b/compose.yaml @@ -9,7 +9,7 @@ services: tags: - ${IMAGE}:latest - ${IMAGE}:dev - command: python -m mir_rest_node --port 3043 --mir_host ${MIR_HOST} + 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 diff --git a/example.env b/example.env index 7466d07..a39a284 100644 --- a/example.env +++ b/example.env @@ -1,5 +1,6 @@ # Note: all paths are relative to the docker compose file MIR_HOST="mirbase2.cels.anl.gov" +MAP_NAME="RPL" WEI_DATA_DIR=~/.wei WORKCELL_FILENAME=test_workcell.yaml WORKCELLS_DIR=./tests/workcell_defs diff --git a/src/mir_rest_node.py b/src/mir_rest_node.py index eac2022..88ed987 100644 --- a/src/mir_rest_node.py +++ b/src/mir_rest_node.py @@ -25,13 +25,18 @@ default="mirbase2.cels.anl.gov", help="Hostname or IP address to connect to MIR Base", ) - +rest_module.arg_parser.add_argument( + "--map_name", + type=str, + default="RPL", + help="Hostname or IP address to connect to MIR Base", +) @rest_module.startup() def mir_startup(state: State): """MIR startup handler.""" state.mir = None - state.mir = MiR_Base(mir_ip=state.mir_host) + state.mir = MiR_Base(mir_ip=state.mir_host, map_name=state.map_name) print("MIR Base online") From 2d46d1e59976e72bff3226e1fde35774c4ed8b30 Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Tue, 20 Aug 2024 23:19:27 -0500 Subject: [PATCH 31/39] tests --- src/mir_driver/locations.json | 2 +- src/mir_driver/mir_driver.py | 22 +++++++++++----------- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/mir_driver/locations.json b/src/mir_driver/locations.json index ac05e2f..3950065 100644 --- a/src/mir_driver/locations.json +++ b/src/mir_driver/locations.json @@ -43,4 +43,4 @@ "type_id": 0 } } -} +} \ No newline at end of file diff --git a/src/mir_driver/mir_driver.py b/src/mir_driver/mir_driver.py index f285745..90b746a 100644 --- a/src/mir_driver/mir_driver.py +++ b/src/mir_driver/mir_driver.py @@ -23,7 +23,7 @@ def __init__( action_dict=None, position_dict=None, curr_mission_queue_id=None, - filename="src/mir_driver/locations.json", + filename="locations.json", ): """ Initialize the MiRBase class with default or provided values. @@ -734,7 +734,7 @@ def wait(self, time): 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 = { @@ -786,12 +786,12 @@ def wait(self, time): # mir_base.move("another_move") # 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) + # 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) From bf5417b48e32724e54087509f846d7ee061ee3f7 Mon Sep 17 00:00:00 2001 From: Dozgulbas Date: Tue, 20 Aug 2024 21:16:25 -0700 Subject: [PATCH 32/39] Clean up --- src/mir_driver/locations.json | 2 +- src/mir_driver/mir_driver.py | 1 - src/mir_rest_node.py | 1 + 3 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/mir_driver/locations.json b/src/mir_driver/locations.json index 3950065..ac05e2f 100644 --- a/src/mir_driver/locations.json +++ b/src/mir_driver/locations.json @@ -43,4 +43,4 @@ "type_id": 0 } } -} \ No newline at end of file +} diff --git a/src/mir_driver/mir_driver.py b/src/mir_driver/mir_driver.py index 90b746a..f8a899a 100644 --- a/src/mir_driver/mir_driver.py +++ b/src/mir_driver/mir_driver.py @@ -5,7 +5,6 @@ import datetime as dt import json -import time from pprint import pprint import requests diff --git a/src/mir_rest_node.py b/src/mir_rest_node.py index 88ed987..9b1fe62 100644 --- a/src/mir_rest_node.py +++ b/src/mir_rest_node.py @@ -32,6 +32,7 @@ help="Hostname or IP address to connect to MIR Base", ) + @rest_module.startup() def mir_startup(state: State): """MIR startup handler.""" From c5afe62a9127e4a7af7a53a133decaf8965d74f8 Mon Sep 17 00:00:00 2001 From: AileenCleary Date: Wed, 21 Aug 2024 10:43:47 -0500 Subject: [PATCH 33/39] Small changes to driver/rest_node to match names --- pyproject.toml | 1 + src/mir_driver/mir_driver.py | 16 ++++++++-------- src/mir_rest_node.py | 6 +++--- 3 files changed, 12 insertions(+), 11 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index 0068686..8e809b8 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -9,6 +9,7 @@ authors = [ {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", diff --git a/src/mir_driver/mir_driver.py b/src/mir_driver/mir_driver.py index f8a899a..57ab025 100644 --- a/src/mir_driver/mir_driver.py +++ b/src/mir_driver/mir_driver.py @@ -676,7 +676,7 @@ def get_state(self): return state.upper() - def move(self, location): + def move(self, location_name): """ Creates a mission to move to a specified location and adds it to the mission queue. @@ -686,16 +686,16 @@ def move(self, location): Returns: dict: The response from posting the mission to the queue. """ - mission_name = f"dock_to_{location}_{dt.datetime.now()}" + 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]["guid"] + 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): + def dock(self, location_name): """ Creates a mission to dock at a specified location and adds it to the mission queue. @@ -705,16 +705,16 @@ def dock(self, location): Returns: dict: The response from posting the mission to the queue. """ - mission_name = f"dock_to_{location}_{dt.datetime.now()}" + 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]["guid"] + guid = data[self.map_name][location_name]["guid"] dock = self.post_mission_to_queue(mission_name, [{"docking": {"marker": guid}}]) return dock - def wait(self, time): + def wait(self, delay_seconds): """ Creates a mission to wait for a specified time and adds it to the mission queue. @@ -724,7 +724,7 @@ def wait(self, time): Returns: dict: The response from posting the mission to the queue. """ - time = str(dt.timedelta(seconds=time)) + 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}}]) diff --git a/src/mir_rest_node.py b/src/mir_rest_node.py index 9b1fe62..5ae5224 100644 --- a/src/mir_rest_node.py +++ b/src/mir_rest_node.py @@ -69,7 +69,7 @@ def move( action: ActionRequest, target_location: Annotated[List[dict], "Target location name"], description: Annotated[str, "Description of the location"], - priority: Annotated[Optional[int], "Prority of the movement in the queue. Default is 1"], + priority: Annotated[Optional[int], "Prority of the movement in the queue. Default is 0."], ) -> StepResponse: """Sends a move command to the MIR Base""" state.move( @@ -126,7 +126,7 @@ def abort_mission_queue( ) -> StepResponse: """Aborts all the missions in the queue""" state.abort_mission_queue() - return StepResponse.step_succeeded("Missions aborted") + return StepResponse.step_succeeded("All pending and executing missions aborted") @rest_module.action( @@ -140,7 +140,7 @@ def add_wait( ) -> StepResponse: """Adds a wait mission to MIR Base""" state.wait(delay_seconds) - return StepResponse.step_succeeded("Missions aborted") + return StepResponse.step_succeeded(f"MIR Base told to wait for {delay_seconds} seconds.") if __name__ == "__main__": From c6281c40fd54e7187d13d975733a494f6cebc7ec Mon Sep 17 00:00:00 2001 From: AileenCleary Date: Wed, 21 Aug 2024 11:19:16 -0500 Subject: [PATCH 34/39] Note on rest node about state messages. TBD --- src/mir_rest_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mir_rest_node.py b/src/mir_rest_node.py index 5ae5224..01e21fc 100644 --- a/src/mir_rest_node.py +++ b/src/mir_rest_node.py @@ -52,7 +52,7 @@ def state(state: State): ] or (state.action_start and (datetime.datetime.now() - state.action_start > datetime.timedelta(0, 2))): robot_state = state.mir.get_state() if robot_state == "ERROR": - state.status = ModuleStatus.ERROR + state.status = ModuleStatus.ERROR # MiR state messages do not align. IDLE should be READY, I believe. Need to run MiR to determine other state messages. elif robot_state == "BUSY": state.status = ModuleStatus.BUSY else: From 505333ca2f036f8fab173932ef9ee0ce5b88b3b4 Mon Sep 17 00:00:00 2001 From: AileenCleary Date: Fri, 30 Aug 2024 14:40:05 -0500 Subject: [PATCH 35/39] Test --- locations.json | 46 ++++++++++++++++++++++++++++++++++++++++++++ src/mir_rest_node.py | 22 ++++++++++----------- 2 files changed, 57 insertions(+), 11 deletions(-) create mode 100644 locations.json 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/src/mir_rest_node.py b/src/mir_rest_node.py index 01e21fc..3b219d0 100644 --- a/src/mir_rest_node.py +++ b/src/mir_rest_node.py @@ -49,7 +49,7 @@ def state(state: State): ModuleStatus.ERROR, ModuleStatus.INIT, None, - ] or (state.action_start and (datetime.datetime.now() - state.action_start > datetime.timedelta(0, 2))): + ] or (state.mir.action_start and (datetime.datetime.now() - state.mir.action_start > datetime.timedelta(0, 2))): robot_state = state.mir.get_state() if robot_state == "ERROR": state.status = ModuleStatus.ERROR # MiR state messages do not align. IDLE should be READY, I believe. Need to run MiR to determine other state messages. @@ -72,10 +72,10 @@ def move( priority: Annotated[Optional[int], "Prority of the movement in the queue. Default is 0."], ) -> StepResponse: """Sends a move command to the MIR Base""" - state.move( + state.mir.move( location_name=target_location, ) - return StepResponse.step_succeeded(f"MIR Base moved to the location: {target_location} ") + return StepResponse.step_succeeded() @rest_module.action( @@ -88,10 +88,10 @@ def dock( target_location: Annotated[List[dict], "Name of the docking location"], ) -> StepResponse: """Sends a docking command to the MIR Base""" - state.dock( + state.mir.dock( location_name=target_location, ) - return StepResponse.step_succeeded(f"MIR Base moved to the location: {target_location} ") + return StepResponse.step_succeeded() @rest_module.action( @@ -107,13 +107,13 @@ def queue_mission( priority: Annotated[Optional[int], "Prority of the mission in the queue. Defult is 1"], ) -> StepResponse: """Sends a mission to the MIR Base which could have multiple movement actions""" - state.post_mission_to_queue( + state.mir.post_mission_to_queue( mission_name=name, act_param_dict=mission, description=description, priority=priority, ) - return StepResponse.step_succeeded(f"Mission {name} is sent to MIR Base") + return StepResponse.step_succeeded() @rest_module.action( @@ -125,8 +125,8 @@ def abort_mission_queue( action: ActionRequest, ) -> StepResponse: """Aborts all the missions in the queue""" - state.abort_mission_queue() - return StepResponse.step_succeeded("All pending and executing missions aborted") + state.mir.abort_mission_queue() + return StepResponse.step_succeeded() @rest_module.action( @@ -139,8 +139,8 @@ def add_wait( delay_seconds: float, ) -> StepResponse: """Adds a wait mission to MIR Base""" - state.wait(delay_seconds) - return StepResponse.step_succeeded(f"MIR Base told to wait for {delay_seconds} seconds.") + state.mir.wait(delay_seconds) + return StepResponse.step_succeeded() if __name__ == "__main__": From 3869cd8d5d4546af72680742bc616ca2b7bbe4a7 Mon Sep 17 00:00:00 2001 From: AileenCleary Date: Thu, 5 Sep 2024 10:55:14 -0500 Subject: [PATCH 36/39] Testing 'wait until finished' action --- src/mir_driver/mir_driver.py | 13 +++++++++++++ src/mir_rest_node.py | 12 ++++++++++++ 2 files changed, 25 insertions(+) diff --git a/src/mir_driver/mir_driver.py b/src/mir_driver/mir_driver.py index 57ab025..6332579 100644 --- a/src/mir_driver/mir_driver.py +++ b/src/mir_driver/mir_driver.py @@ -5,6 +5,7 @@ import datetime as dt import json +import time from pprint import pprint import requests @@ -365,6 +366,18 @@ def post_mission_to_queue(self, mission_name, act_param_dict, description="", pr return response + 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. + """ + mission_finished = self.get_mission_queue()[-1]["state"] + while mission_finished != "Done": + mission_finished = self.get_mission_queue()[-1]["state"] + time.sleep(5) + + return + def check_queue_completion(self, printq=False): """ Check and print the status of the current mission queue and its actions. diff --git a/src/mir_rest_node.py b/src/mir_rest_node.py index 3b219d0..60c1091 100644 --- a/src/mir_rest_node.py +++ b/src/mir_rest_node.py @@ -78,6 +78,18 @@ def move( return StepResponse.step_succeeded() +@rest_module.action( + name="wait_until_finished", description="Wait until previous mission is finished before proceeding." +) +def wait_until_finished( + state: State, + action: ActionRequest, +) -> StepResponse: + """MIR Base continuously checks status of last sent mission before proceeding.""" + state.mir.wait_until_finished() + return StepResponse.step_succeeded() + + @rest_module.action( name="dock", description="Sends a dock command to the MIR Base", From bab8c675b2f6fdb5effbb718ad082618d1dbf6da Mon Sep 17 00:00:00 2001 From: AileenCleary Date: Fri, 6 Sep 2024 13:17:22 -0500 Subject: [PATCH 37/39] Changed state handler. Added 'Executing' state for executing missions.. Need to test in person --- src/mir_driver/mir_driver.py | 8 ++++++-- src/mir_rest_node.py | 23 ++++++++++++++--------- 2 files changed, 20 insertions(+), 11 deletions(-) diff --git a/src/mir_driver/mir_driver.py b/src/mir_driver/mir_driver.py index 6332579..a3ebdfd 100644 --- a/src/mir_driver/mir_driver.py +++ b/src/mir_driver/mir_driver.py @@ -24,6 +24,7 @@ def __init__( position_dict=None, curr_mission_queue_id=None, filename="locations.json", + status=None, ): """ Initialize the MiRBase class with default or provided values. @@ -46,6 +47,7 @@ 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): """ @@ -371,11 +373,12 @@ 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 def check_queue_completion(self, printq=False): @@ -414,7 +417,7 @@ def check_queue_completion(self, printq=False): return - def status(self): + def self_status(self): """ Retrieves the current system status of the MiR Robotic base. @@ -686,6 +689,7 @@ def get_state(self): url = "status/?whitelist=state_text" state = self.receive_response(url, False).get("state_text") print(state.upper()) + self.status = state.upper() return state.upper() diff --git a/src/mir_rest_node.py b/src/mir_rest_node.py index 60c1091..a004126 100644 --- a/src/mir_rest_node.py +++ b/src/mir_rest_node.py @@ -1,6 +1,5 @@ """REST-based node for UR robots""" -import datetime from pathlib import Path from typing import List, Optional @@ -42,21 +41,27 @@ def mir_startup(state: State): @rest_module.state_handler() -def state(state: State): +def state( + state: State, +): # ** TBD, added "EXECUTING" state to "ModuleStatus" because MiR can be ready to accept missions but also executing them. Need to test if this works. """Returns the current state of the UR module""" if state.status not in [ - ModuleStatus.BUSY, ModuleStatus.ERROR, ModuleStatus.INIT, None, - ] or (state.mir.action_start and (datetime.datetime.now() - state.mir.action_start > datetime.timedelta(0, 2))): - robot_state = state.mir.get_state() - if robot_state == "ERROR": - state.status = ModuleStatus.ERROR # MiR state messages do not align. IDLE should be READY, I believe. Need to run MiR to determine other state messages. - elif robot_state == "BUSY": + ]: + if state.mir.status == "BUSY": state.status = ModuleStatus.BUSY - else: + elif state.mir.status == "IDLE": state.status = ModuleStatus.IDLE + else: + state.mir.status = state.mir.get_state() + if state.mir.status in ["READY", "IDLE"]: + state.status = ModuleStatus.IDLE + elif state.mir.status in ["PENDING", "EXECUTING"]: + state.status = ModuleStatus.EXECUTING + else: + state.status = ModuleStatus.ERROR return ModuleState(status=state.status, error="") From 2f3dcdb3452ce6e8d1bad76e71de8369c04b1992 Mon Sep 17 00:00:00 2001 From: AileenCleary Date: Wed, 18 Sep 2024 10:44:18 -0500 Subject: [PATCH 38/39] More specific state messages. --- README.md | 24 +++++++++++++++++++++++- src/mir_driver/mir_driver.py | 5 ++++- src/mir_rest_node.py | 33 ++++++++++++++------------------- 3 files changed, 41 insertions(+), 21 deletions(-) diff --git a/README.md b/README.md index 9dba7bb..55f4559 100644 --- a/README.md +++ b/README.md @@ -1 +1,23 @@ -# mir_module +# 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/src/mir_driver/mir_driver.py b/src/mir_driver/mir_driver.py index a3ebdfd..807b9f3 100644 --- a/src/mir_driver/mir_driver.py +++ b/src/mir_driver/mir_driver.py @@ -689,7 +689,6 @@ def get_state(self): url = "status/?whitelist=state_text" state = self.receive_response(url, False).get("state_text") print(state.upper()) - self.status = state.upper() return state.upper() @@ -800,6 +799,10 @@ def wait(self, delay_seconds): # 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() diff --git a/src/mir_rest_node.py b/src/mir_rest_node.py index a004126..f629c55 100644 --- a/src/mir_rest_node.py +++ b/src/mir_rest_node.py @@ -41,28 +41,23 @@ def mir_startup(state: State): @rest_module.state_handler() -def state( - state: State, -): # ** TBD, added "EXECUTING" state to "ModuleStatus" because MiR can be ready to accept missions but also executing them. Need to test if this works. +def state(state: State): """Returns the current state of the UR module""" - if state.status not in [ - ModuleStatus.ERROR, - ModuleStatus.INIT, - None, - ]: - if state.mir.status == "BUSY": - state.status = ModuleStatus.BUSY - elif state.mir.status == "IDLE": + if state.mir.status != "BUSY": + robot_state = state.mir.get_state() + if robot_state == "ERROR": + state.status = ModuleStatus.ERROR # MiR state messages do not align. IDLE should be READY, I believe. Need to run MiR to determine other state messages. + error = "" + elif robot_state == "EXECUTING": state.status = ModuleStatus.IDLE + error = "Executing current mission, can still accept more missions." else: - state.mir.status = state.mir.get_state() - if state.mir.status in ["READY", "IDLE"]: - state.status = ModuleStatus.IDLE - elif state.mir.status in ["PENDING", "EXECUTING"]: - state.status = ModuleStatus.EXECUTING - else: - state.status = ModuleStatus.ERROR - return ModuleState(status=state.status, error="") + state.status = ModuleStatus.IDLE + error = "All missions complete." + else: + state.status = ModuleStatus.BUSY + error = "Waiting for other modules to finish." + return ModuleState(status=state.status, error=error) @rest_module.action( From 838424ff70930d783caa336d924eb9129afb3141 Mon Sep 17 00:00:00 2001 From: tginsbu1 Date: Wed, 25 Mar 2026 02:44:35 -0500 Subject: [PATCH 39/39] updated to madsci --- src/mir_rest_node.py | 247 +++++++++++++++++-------------------------- 1 file changed, 98 insertions(+), 149 deletions(-) diff --git a/src/mir_rest_node.py b/src/mir_rest_node.py index f629c55..18bd2f1 100644 --- a/src/mir_rest_node.py +++ b/src/mir_rest_node.py @@ -3,157 +3,106 @@ from pathlib import Path from typing import List, Optional -from fastapi.datastructures import State from mir_driver.mir_driver import MiR_Base from typing_extensions import Annotated -from wei.modules.rest_module import RESTModule -from wei.types.module_types import ModuleState, ModuleStatus -from wei.types.step_types import ActionRequest, StepResponse -from wei.utils import extract_version - -rest_module = RESTModule( - name="mir_node", - version=extract_version(Path(__file__).parent.parent / "pyproject.toml"), - description="A node to control the mobile MIR Base", - model="Mir250", -) - -rest_module.arg_parser.add_argument( - "--mir_host", - type=str, - default="mirbase2.cels.anl.gov", - help="Hostname or IP address to connect to MIR Base", -) -rest_module.arg_parser.add_argument( - "--map_name", - type=str, - default="RPL", - help="Hostname or IP address to connect to MIR Base", -) - - -@rest_module.startup() -def mir_startup(state: State): - """MIR startup handler.""" - state.mir = None - state.mir = MiR_Base(mir_ip=state.mir_host, map_name=state.map_name) - print("MIR Base online") - - -@rest_module.state_handler() -def state(state: State): - """Returns the current state of the UR module""" - if state.mir.status != "BUSY": - robot_state = state.mir.get_state() - if robot_state == "ERROR": - state.status = ModuleStatus.ERROR # MiR state messages do not align. IDLE should be READY, I believe. Need to run MiR to determine other state messages. - error = "" - elif robot_state == "EXECUTING": - state.status = ModuleStatus.IDLE - error = "Executing current mission, can still accept more missions." - else: - state.status = ModuleStatus.IDLE - error = "All missions complete." - else: - state.status = ModuleStatus.BUSY - error = "Waiting for other modules to finish." - return ModuleState(status=state.status, error=error) - - -@rest_module.action( - name="move", - description="Send a Move command to the MIR Base", -) -def move( - state: State, - action: ActionRequest, - target_location: Annotated[List[dict], "Target location name"], - description: Annotated[str, "Description of the location"], - priority: Annotated[Optional[int], "Prority of the movement in the queue. Default is 0."], -) -> StepResponse: - """Sends a move command to the MIR Base""" - state.mir.move( - location_name=target_location, - ) - return StepResponse.step_succeeded() - - -@rest_module.action( - name="wait_until_finished", description="Wait until previous mission is finished before proceeding." -) -def wait_until_finished( - state: State, - action: ActionRequest, -) -> StepResponse: - """MIR Base continuously checks status of last sent mission before proceeding.""" - state.mir.wait_until_finished() - return StepResponse.step_succeeded() - - -@rest_module.action( - name="dock", - description="Sends a dock command to the MIR Base", -) -def dock( - state: State, - action: ActionRequest, - target_location: Annotated[List[dict], "Name of the docking location"], -) -> StepResponse: - """Sends a docking command to the MIR Base""" - state.mir.dock( - location_name=target_location, - ) - return StepResponse.step_succeeded() - - -@rest_module.action( - name="queue_mission", - description="Adds a new mission to the queue. A mission could have multiple movement actions", -) -def queue_mission( - state: State, - action: ActionRequest, - 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"], -) -> StepResponse: - """Sends a mission to the MIR Base which could have multiple movement actions""" - state.mir.post_mission_to_queue( - mission_name=name, - act_param_dict=mission, - description=description, - priority=priority, - ) - return StepResponse.step_succeeded() - - -@rest_module.action( - name="abort_mission_queue", - description="Send a abort_mission_queue command to the MIR Base", -) -def abort_mission_queue( - state: State, - action: ActionRequest, -) -> StepResponse: - """Aborts all the missions in the queue""" - state.mir.abort_mission_queue() - return StepResponse.step_succeeded() - - -@rest_module.action( - name="add_wait", - description="Send a abort_mission_queue command to the MIR Base", -) -def add_wait( - state: State, - action: ActionRequest, - delay_seconds: float, -) -> StepResponse: - """Adds a wait mission to MIR Base""" - state.mir.wait(delay_seconds) - return StepResponse.step_succeeded() +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__": - rest_module.start() + MIR_node = MIRNode() + MIR_node.start_node