Esempio n. 1
0
    def submit_task_request(self, req_json) -> Tuple[str, str]:
        """
        Task Submission - This function will trigger a ros srv call to the
        dispatcher node, and return a response. Function will return a Task ID

        Args:
            req_json: task description in json format
        Returns:
            task_id, error_msg: if submission failed
        """
        # construct task request json from legacy format
        request_json, err_msg = self.__convert_task_description(req_json)
        if request_json is None:
            self.get_logger().error(err_msg)
            return "", err_msg
        payload = {"type": "dispatch_task_request", "request": request_json}

        msg = ApiRequest()
        msg.request_id = "demos_" + str(uuid.uuid4())
        msg.json_msg = json.dumps(payload)
        self.task_api_req_pub.publish(msg)
        self.get_logger().info(f'Publish task request {msg}')

        # Note: API Response or can wait for response
        # TODO: listen to "/task_api_responses"
        return msg.request_id, ""  # success
Esempio n. 2
0
    def __init__(self, argv=sys.argv):
        super().__init__('task_requester')
        parser = argparse.ArgumentParser()
        parser.add_argument('-id',
                            '--task_id',
                            required=True,
                            default='',
                            type=str,
                            help='Cancel Task ID')

        self.args = parser.parse_args(argv[1:])

        transient_qos = QoSProfile(history=History.KEEP_LAST,
                                   depth=1,
                                   reliability=Reliability.RELIABLE,
                                   durability=Durability.TRANSIENT_LOCAL)

        self.pub = self.create_publisher(ApiRequest, 'task_api_requests',
                                         transient_qos)

        # Construct task
        msg = ApiRequest()
        msg.request_id = "cancel_task_" + str(uuid.uuid4())
        payload = {}
        payload["type"] = "cancel_task_request"
        payload["task_id"] = self.args.task_id

        msg.json_msg = json.dumps(payload)
        print(f"msg: \n{json.dumps(payload, indent=2)}")
        self.pub.publish(msg)
Esempio n. 3
0
    def cancel_task_request(self, task_id) -> bool:
        """
        Cancel Task - This function will trigger a ros srv call to the
        dispatcher node, and return a response.
        """
        print(f"Canceling Task Request! {task_id}")
        payload = {
            "type": "cancel_task_request",
            "task_id": task_id,
            "labels": ["cancellation from simple api server"]
        }
        msg = ApiRequest()
        msg.request_id = "demos_" + str(uuid.uuid4())
        msg.json_msg = json.dumps(payload)
        # self.task_api_req_pub.publish(msg)

        # TODO: check res from "/task_api_responses"
        #   cancellation is not fully tested in "rmf_ros2"
        return False
Esempio n. 4
0
    def __init__(self, argv=sys.argv):
        super().__init__('task_requester')
        parser = argparse.ArgumentParser()
        parser.add_argument('-p',
                            '--places',
                            required=True,
                            nargs='+',
                            type=str,
                            help='Places to patrol through')
        parser.add_argument('-n',
                            '--rounds',
                            help='Number of loops to perform',
                            type=int,
                            default=1)
        parser.add_argument('-F',
                            '--fleet',
                            type=str,
                            help='Fleet name, should define tgt with robot')
        parser.add_argument('-R',
                            '--robot',
                            type=str,
                            help='Robot name, should define tgt with fleet')
        parser.add_argument('-st',
                            '--start_time',
                            help='Start time from now in secs, default: 0',
                            type=int,
                            default=0)
        parser.add_argument('-pt',
                            '--priority',
                            help='Priority value for this request',
                            type=int,
                            default=0)
        parser.add_argument("--use_sim_time",
                            action="store_true",
                            help='Use sim time, default: false')

        self.args = parser.parse_args(argv[1:])
        self.response = asyncio.Future()

        transient_qos = QoSProfile(history=History.KEEP_LAST,
                                   depth=1,
                                   reliability=Reliability.RELIABLE,
                                   durability=Durability.TRANSIENT_LOCAL)
        self.pub = self.create_publisher(ApiRequest, 'task_api_requests',
                                         transient_qos)

        # enable ros sim time
        if self.args.use_sim_time:
            self.get_logger().info("Using Sim Time")
            param = Parameter("use_sim_time", Parameter.Type.BOOL, True)
            self.set_parameters([param])

        # Construct task
        msg = ApiRequest()
        msg.request_id = "patrol_" + str(uuid.uuid4())
        payload = {}

        if self.args.robot and self.args.fleet:
            self.get_logger().info("Using 'robot_task_request'")
            payload["type"] = "robot_task_request"
            payload["robot"] = self.args.robot
            payload["fleet"] = self.args.fleet
        else:
            self.get_logger().info("Using 'dispatch_task_request'")
            payload["type"] = "dispatch_task_request"

        request = {}

        # Set task request start time
        now = self.get_clock().now().to_msg()
        now.sec = now.sec + self.args.start_time
        start_time = now.sec * 1000 + round(now.nanosec / 10**6)
        request["unix_millis_earliest_start_time"] = start_time
        # todo(YV): Fill priority after schema is added

        # Define task request category
        request["category"] = "patrol"

        # Define task request description
        description = {'places': self.args.places, 'rounds': self.args.rounds}
        request["description"] = description
        payload["request"] = request
        msg.json_msg = json.dumps(payload)

        def receive_response(response_msg: ApiResponse):
            if response_msg.request_id == msg.request_id:
                self.response.set_result(json.loads(response_msg.json_msg))

        self.sub = self.create_subscription(ApiResponse, 'task_api_responses',
                                            receive_response, 10)

        print(f"Json msg payload: \n{json.dumps(payload, indent=2)}")
        self.pub.publish(msg)
Esempio n. 5
0
    def __init__(self, argv=sys.argv):
        super().__init__('task_requester')
        parser = argparse.ArgumentParser()
        parser.add_argument('-s', '--starts', default=[],
                            type=str, nargs='+', help='Action start waypoints')
        parser.add_argument('-st', '--start_time',
                            help='Start time from now in secs, default: 0',
                            type=int, default=0)
        parser.add_argument('-pt', '--priority',
                            help='Priority value for this request',
                            type=int, default=0)
        parser.add_argument('-a', '--action', required=True,
                            type=str, help='action name')
        parser.add_argument('-ad', '--action_desc', required=False,
                            default='{}',
                            type=str, help='action description in json str')
        parser.add_argument('-F', '--fleet', type=str,
                            help='Fleet name, should define tgt with robot')
        parser.add_argument('-R', '--robot', type=str,
                            help='Robot name, should define tgt with fleet')
        parser.add_argument("--use_sim_time", action="store_true",
                            help='Use sim time, default: false')
        parser.add_argument("--use_tool_sink", action="store_true",
                            help='Use tool sink during perform action, \
                                default: false')

        self.args = parser.parse_args(argv[1:])
        self.response = asyncio.Future()

        transient_qos = QoSProfile(
            history=History.KEEP_LAST,
            depth=1,
            reliability=Reliability.RELIABLE,
            durability=Durability.TRANSIENT_LOCAL)

        self.pub = self.create_publisher(
          ApiRequest, 'task_api_requests', transient_qos)

        # enable ros sim time
        if self.args.use_sim_time:
            self.get_logger().info("Using Sim Time")
            param = Parameter("use_sim_time", Parameter.Type.BOOL, True)
            self.set_parameters([param])

        # Construct task
        msg = ApiRequest()
        msg.request_id = "custom_action_" + str(uuid.uuid4())
        payload = {}
        if self.args.fleet and self.args.robot:
            self.get_logger().info("Using 'robot_task_request'")
            payload["type"] = "robot_task_request"
            payload["robot"] = self.args.robot
            payload["fleet"] = self.args.fleet
        else:
            self.get_logger().info("Using 'dispatch_task_request'")
            payload["type"] = "dispatch_task_request"
        request = {}

        # Set task request start time
        now = self.get_clock().now().to_msg()
        now.sec = now.sec + self.args.start_time
        start_time = now.sec * 1000 + round(now.nanosec/10**6)
        request["unix_millis_earliest_start_time"] = start_time
        # todo(YV): Fill priority after schema is added

        # Define task request category
        request["category"] = "compose"

        # Define task request description with phases
        description = {}  # task_description_Compose.json
        description["category"] = self.args.action
        description["phases"] = []
        activities = []

        def _add_action():
            activities.append(
                {
                    "category": "perform_action",
                    "description":
                    {
                        "unix_millis_action_duration_estimate": 60000,
                        "category": self.args.action,
                        "description": json.loads(self.args.action_desc),
                        "use_tool_sink": self.args.use_tool_sink
                    }
                })

        if not self.args.starts:
            _add_action()
        else:
            # Add action activities
            for start in self.args.starts:
                activities.append({
                        "category": "go_to_place",
                        "description": start
                    })
                _add_action()

        # Add activities to phases
        description["phases"].append({
                "activity": {
                    "category": "sequence",
                    "description": {"activities": activities}}
            })

        request["description"] = description
        payload["request"] = request
        msg.json_msg = json.dumps(payload)

        def receive_response(response_msg: ApiResponse):
            if response_msg.request_id == msg.request_id:
                self.response.set_result(json.loads(response_msg.json_msg))

        self.sub = self.create_subscription(
            ApiResponse, 'task_api_responses', receive_response, 10
        )

        print(f"msg: \n{json.dumps(payload, indent=2)}")
        self.pub.publish(msg)
Esempio n. 6
0
    def __init__(self, argv=sys.argv):
        super().__init__('task_requester')
        parser = argparse.ArgumentParser()
        parser.add_argument('-p', '--pickups', required=True,
                            type=str, nargs='+',
                            help="Pickup names")
        parser.add_argument('-d', '--dropoffs', required=True,
                            type=str, nargs='+',
                            help="Dropoff names")
        parser.add_argument('-ph', '--pickup_handlers', required=True,
                            type=str, nargs='+',
                            help="Pickup handler names")
        parser.add_argument('-dh', '--dropoff_handlers', required=True,
                            type=str, nargs='+',
                            help="Dropoffs handler names")
        parser.add_argument('-pp', '--pickup_payloads',
                            type=str, nargs='+', default=[],
                            help="Pickup payload [sku,quantity sku2,qty...]")
        parser.add_argument('-dp', '--dropoff_payloads',
                            type=str, nargs='+', default=[],
                            help="Dropoff payload [sku,quantity sku2,qty...]")
        parser.add_argument('-F', '--fleet', type=str,
                            help='Fleet name, should define tgt with robot')
        parser.add_argument('-R', '--robot', type=str,
                            help='Robot name, should define tgt with fleet')
        parser.add_argument('-st', '--start_time',
                            help='Start time from now in secs, default: 0',
                            type=int, default=0)
        parser.add_argument('-pt', '--priority',
                            help='Priority value for this request',
                            type=int, default=0)
        parser.add_argument("--use_sim_time", action="store_true",
                            help='Use sim time, default: false')

        self.args = parser.parse_args(argv[1:])
        self.response = asyncio.Future()

        # check user delivery arg inputs
        if (len(self.args.pickups) != len(self.args.pickup_handlers)):
            self.get_logger().error(
                "Invalid pickups, [-p] should have the same length as [-ph]")
            parser.print_help()
            sys.exit(1)
        if (len(self.args.dropoffs) != len(self.args.dropoff_handlers)):
            self.get_logger().error(
                "Invalid dropoffs, [-d] should have the same length as [-dh]")
            parser.print_help()
            sys.exit(1)

        transient_qos = QoSProfile(
            history=History.KEEP_LAST,
            depth=1,
            reliability=Reliability.RELIABLE,
            durability=Durability.TRANSIENT_LOCAL)

        self.pub = self.create_publisher(
          ApiRequest, 'task_api_requests', transient_qos)

        # enable ros sim time
        if self.args.use_sim_time:
            self.get_logger().info("Using Sim Time")
            param = Parameter("use_sim_time", Parameter.Type.BOOL, True)
            self.set_parameters([param])

        # Construct task
        msg = ApiRequest()
        msg.request_id = "delivery_" + str(uuid.uuid4())
        payload = {}
        if self.args.fleet and self.args.robot:
            self.get_logger().info("Using 'robot_task_request'")
            payload["type"] = "robot_task_request"
            payload["robot"] = self.args.robot
            payload["fleet"] = self.args.fleet
        else:
            self.get_logger().info("Using 'dispatch_task_request'")
            payload["type"] = "dispatch_task_request"
        request = {}

        # Set task request start time
        now = self.get_clock().now().to_msg()
        now.sec = now.sec + self.args.start_time
        start_time = now.sec * 1000 + round(now.nanosec/10**6)
        request["unix_millis_earliest_start_time"] = start_time

        def __create_pickup_desc(index):
            if index < len(self.args.pickup_payloads):
                sku_qty = self.args.pickup_payloads[index].split(',')
                assert len(sku_qty) == 2, \
                    "please specify sku and qty for pickup payload"
                payload = [{"sku": sku_qty[0],
                            "quantity": int(sku_qty[1])}]
            else:
                payload = []

            return {
                    "place": self.args.pickups[index],
                    "handler": self.args.pickup_handlers[index],
                    "payload": payload
                    }

        def __create_dropoff_desc(index):
            if index < len(self.args.dropoff_payloads):
                sku_qty = self.args.dropoff_payloads[index].split(',')
                assert len(sku_qty) == 2, \
                    "please specify sku and qty for dropoff payload"
                payload = [{"sku": sku_qty[0],
                            "quantity": int(sku_qty[1])}]
            else:
                payload = []

            return {
                    "place": self.args.dropoffs[index],
                    "handler": self.args.dropoff_handlers[index],
                    "payload": payload
                    }

        # Use standard delivery task type
        if len(self.args.pickups) == 1 and len(self.args.dropoffs) == 1:
            request["category"] = "delivery"
            description = {
                "pickup": __create_pickup_desc(0),
                "dropoff": __create_dropoff_desc(0)
                }
        else:
            # Define multi_delivery with request category compose
            request["category"] = "compose"

            # Define task request description with phases
            description = {}  # task_description_Compose.json
            description["category"] = "multi_delivery"
            description["phases"] = []
            activities = []
            # Add each pickup
            for i in range(0, len(self.args.pickups)):
                activities.append({
                    "category": "pickup",
                    "description": __create_pickup_desc(i)})
            # Add each dropoff
            for i in range(0, len(self.args.dropoffs)):
                activities.append({
                    "category": "dropoff",
                    "description": __create_dropoff_desc(i)})
            # Add activities to phases
            description["phases"].append(
                {"activity": {
                    "category": "sequence",
                    "description": {"activities": activities}}})

        request["description"] = description
        payload["request"] = request
        msg.json_msg = json.dumps(payload)

        def receive_response(response_msg: ApiResponse):
            if response_msg.request_id == msg.request_id:
                self.response.set_result(json.loads(response_msg.json_msg))

        self.sub = self.create_subscription(
            ApiResponse, 'task_api_responses', receive_response, 10
        )

        print(f"Json msg payload: \n{json.dumps(payload, indent=2)}")

        self.pub.publish(msg)
Esempio n. 7
0
    def __init__(self, argv=sys.argv):
        super().__init__('task_requester')
        parser = argparse.ArgumentParser()
        parser.add_argument('-F', '--fleet', type=str, help='Fleet name')
        parser.add_argument('-R', '--robot', type=str, help='Robot name')
        parser.add_argument('-p',
                            '--place',
                            required=True,
                            type=str,
                            help='Place to go to')
        parser.add_argument('-o',
                            '--orientation',
                            required=False,
                            type=float,
                            help='Orientation to face in degrees (optional)')
        parser.add_argument('-st',
                            '--start_time',
                            help='Start time from now in secs, default: 0',
                            type=int,
                            default=0)
        parser.add_argument('-pt',
                            '--priority',
                            help='Priority value for this request',
                            type=int,
                            default=0)
        parser.add_argument("--use_sim_time",
                            action="store_true",
                            help='Use sim time, default: false')

        self.args = parser.parse_args(argv[1:])
        self.response = asyncio.Future()

        transient_qos = QoSProfile(history=History.KEEP_LAST,
                                   depth=1,
                                   reliability=Reliability.RELIABLE,
                                   durability=Durability.TRANSIENT_LOCAL)

        self.pub = self.create_publisher(ApiRequest, 'task_api_requests',
                                         transient_qos)

        # enable ros sim time
        if self.args.use_sim_time:
            self.get_logger().info("Using Sim Time")
            param = Parameter("use_sim_time", Parameter.Type.BOOL, True)
            self.set_parameters([param])

        # Construct task
        msg = ApiRequest()
        msg.request_id = "direct_" + str(uuid.uuid4())
        payload = {}

        if self.args.robot and self.args.fleet:
            self.get_logger().info("Using 'robot_task_request'")
            payload["type"] = "robot_task_request"
            payload["robot"] = self.args.robot
            payload["fleet"] = self.args.fleet
        else:
            self.get_logger().info("Using 'dispatch_task_request'")
            payload["type"] = "dispatch_task_request"

        # Set task request start time
        now = self.get_clock().now().to_msg()
        now.sec = now.sec + self.args.start_time
        start_time = now.sec * 1000 + round(now.nanosec / 10**6)
        # todo(YV): Fill priority after schema is added

        # Define task request description
        go_to_description = {'waypoint': self.args.place}
        if self.args.orientation is not None:
            go_to_description['orientation'] = (self.args.orientation *
                                                math.pi / 180.0)

        go_to_activity = {
            'category': 'go_to_place',
            'description': go_to_description
        }

        rmf_task_request = {
            'category': 'compose',
            'description': {
                'category': 'go_to_place',
                'phases': [{
                    'activity': go_to_activity
                }]
            },
            'unix_millis_earliest_start_time': start_time
        }

        payload["request"] = rmf_task_request

        msg.json_msg = json.dumps(payload)

        def receive_response(response_msg: ApiResponse):
            if response_msg.request_id == msg.request_id:
                self.response.set_result(json.loads(response_msg.json_msg))

        self.sub = self.create_subscription(ApiResponse, 'task_api_responses',
                                            receive_response, 10)

        print(f"Json msg payload: \n{json.dumps(payload, indent=2)}")

        self.pub.publish(msg)
Esempio n. 8
0
    def __init__(self, argv=sys.argv):
        super().__init__('task_requester')
        parser = argparse.ArgumentParser()
        parser.add_argument('-F',
                            '--fleet',
                            required=False,
                            default='',
                            type=str,
                            help='Fleet name')
        parser.add_argument('-R',
                            '--robot',
                            required=False,
                            default='',
                            type=str,
                            help='Robot name')
        parser.add_argument('-s',
                            '--start',
                            required=True,
                            type=str,
                            help='Start waypoint')
        parser.add_argument('-st',
                            '--start_time',
                            help='Start time from now in secs, default: 0',
                            type=int,
                            default=0)
        parser.add_argument('-pt',
                            '--priority',
                            help='Priority value for this request',
                            type=int,
                            default=0)
        parser.add_argument("--use_sim_time",
                            action="store_true",
                            help='Use sim time, default: false')

        self.args = parser.parse_args(argv[1:])

        transient_qos = QoSProfile(history=History.KEEP_LAST,
                                   depth=1,
                                   reliability=Reliability.RELIABLE,
                                   durability=Durability.TRANSIENT_LOCAL)

        self.pub = self.create_publisher(ApiRequest, 'task_api_requests',
                                         transient_qos)

        # enable ros sim time
        if self.args.use_sim_time:
            self.get_logger().info("Using Sim Time")
            param = Parameter("use_sim_time", Parameter.Type.BOOL, True)
            self.set_parameters([param])

        # Construct task
        msg = ApiRequest()
        msg.request_id = "teleop_" + str(uuid.uuid4())
        payload = {}
        if self.args.fleet and self.args.robot:
            payload["type"] = "robot_task_request"
            payload["robot"] = self.args.robot
            payload["fleet"] = self.args.fleet
        else:
            payload["type"] = "dispatch_task_request"
        request = {}

        # Set task request start time
        now = self.get_clock().now().to_msg()
        now.sec = now.sec + self.args.start_time
        start_time = now.sec * 1000 + round(now.nanosec / 10**6)
        request["unix_millis_earliest_start_time"] = start_time
        # todo(YV): Fill priority after schema is added

        # Define task request category
        request["category"] = "compose"

        # Define task request description with phases
        description = {}  # task_description_Compose.json
        description["category"] = "teleop"
        description["phases"] = []
        activities = []
        # Add activities
        activities.append({
            "category": "go_to_place",
            "description": self.args.start
        })
        activities.append({
            "category": "perform_action",
            "description": {
                "unix_millis_action_duration_estimate": 60000,
                "category": "teleop",
                "description": {}
            }
        })
        # Add activities to phases
        description["phases"].append({
            "activity": {
                "category": "sequence",
                "description": {
                    "activities": activities
                }
            }
        })
        request["description"] = description
        payload["request"] = request
        msg.json_msg = json.dumps(payload)
        print(f"Json msg payload: \n{json.dumps(payload, indent=2)}")
        self.pub.publish(msg)