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
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)
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
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)
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)
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)
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)
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)