def main():
# INIT RCL ================================================================
rclpy.init()
adpt.init_rclcpp()
# INIT GRAPH ==============================================================
map_name = "test_map"
test_graph = graph.Graph()
test_graph.add_waypoint(map_name, [0.0, -10.0]) # 0
test_graph.add_waypoint(map_name, [0.0, -5.0]) # 1
test_graph.add_waypoint(map_name, [5.0, -5.0]).set_holding_point(True) # 2
test_graph.add_waypoint(map_name, [-10.0, 0]) # 3
test_graph.add_waypoint(map_name, [-5.0, 0.0]) # 4
test_graph.add_waypoint(map_name, [0.0, 0.0]) # 5
test_graph.add_waypoint(map_name, [5.0, 0.0]) # 6
test_graph.add_waypoint(map_name, [10.0, 0.0]) # 7
test_graph.add_waypoint(map_name, [0.0, 5.0]) # 8
test_graph.add_waypoint(map_name, [5.0, 5.0]).set_holding_point(True) # 9
test_graph.add_waypoint(map_name, [0.0, 10.0]) # 10
assert test_graph.get_waypoint(2).holding_point
assert test_graph.get_waypoint(9).holding_point
assert not test_graph.get_waypoint(10).holding_point
"""
10(D)
|
|
8------9
| |
| |
3------4------5------6------7(D)
| |
| |
1------2
|
|
0
"""
test_graph.add_bidir_lane(0, 1) # 0 1
test_graph.add_bidir_lane(1, 2) # 2 3
test_graph.add_bidir_lane(1, 5) # 4 5
test_graph.add_bidir_lane(2, 6) # 6 7
test_graph.add_bidir_lane(3, 4) # 8 9
test_graph.add_bidir_lane(4, 5) # 10 11
test_graph.add_bidir_lane(5, 6) # 12 13
test_graph.add_dock_lane(6, 7, "A") # 14 15
test_graph.add_bidir_lane(5, 8) # 16 17
test_graph.add_bidir_lane(6, 9) # 18 19
test_graph.add_bidir_lane(8, 9) # 20 21
test_graph.add_dock_lane(8, 10, "B") # 22 23
assert test_graph.num_lanes == 24
test_graph.add_key(pickup_name, 7)
test_graph.add_key(dropoff_name, 10)
assert len(test_graph.keys) == 2 and pickup_name in test_graph.keys \
and dropoff_name in test_graph.keys
# INIT FLEET ==============================================================
profile = traits.Profile(geometry.make_final_convex_circle(1.0))
robot_traits = traits.VehicleTraits(linear=traits.Limits(0.7, 0.3),
angular=traits.Limits(1.0, 0.45),
profile=profile)
# Manages delivery or loop requests
adapter = adpt.MockAdapter("TestDeliveryAdapter")
fleet = adapter.add_fleet("test_fleet", robot_traits, test_graph)
fleet.accept_delivery_requests(lambda x: True)
cmd_node = Node("RobotCommandHandle")
# Test compute_plan_starts, which tries to place the robot on the navgraph
# Your robot MUST be near a waypoint or lane for this to work though!
starts = plan.compute_plan_starts(test_graph, "test_map",
[[-10.0], [0.0], [0.0]], adapter.now())
assert [x.waypoint for x in starts] == [3], [x.waypoint for x in starts]
# Alternatively, if you DO know where your robot is, place it directly!
starts = [plan.Start(adapter.now(), 0, 0.0)]
# Lambda to insert an adapter
def updater_inserter(handle_obj, updater):
handle_obj.updater = updater
# Manages and executes robot commands
robot_cmd = MockRobotCommand(cmd_node, test_graph)
fleet.add_robot(robot_cmd, "T0", profile, starts,
partial(updater_inserter, robot_cmd))
# INIT TASK OBSERVER ======================================================
at_least_one_incomplete = False
completed = False
def task_cb(msg):
nonlocal at_least_one_incomplete
nonlocal completed
if msg.state == TASK_STATE_COMPLETED:
completed = True
at_least_one_incomplete = False
else:
completed = False
at_least_one_incomplete = True
task_node = rclpy.create_node("task_summary_node")
task_node.create_subscription(TaskSummary, "task_summaries", task_cb, 10)
# INIT DISPENSERS =========================================================
quiet_dispenser = MockQuietDispenser(quiet_dispenser_name)
flaky_dispenser = MockFlakyDispenser(flaky_dispenser_name)
# FINAL PREP ==============================================================
rclpy_executor = SingleThreadedExecutor()
rclpy_executor.add_node(task_node)
rclpy_executor.add_node(cmd_node)
rclpy_executor.add_node(quiet_dispenser)
rclpy_executor.add_node(flaky_dispenser)
last_quiet_state = False
last_flaky_state = False
# GO! =====================================================================
adapter.start()
print("# SENDING NEW REQUEST ############################################")
request = adpt.type.CPPDeliveryMsg("test_delivery", pickup_name,
quiet_dispenser_name, dropoff_name,
flaky_dispenser_name)
quiet_dispenser.reset()
flaky_dispenser.reset()
adapter.request_delivery(request)
rclpy_executor.spin_once(1)
for i in range(1000):
if quiet_dispenser.success_flag != last_quiet_state:
last_quiet_state = quiet_dispenser.success_flag
print("== QUIET DISPENSER FLIPPED SUCCESS STATE ==",
last_quiet_state)
if flaky_dispenser.success_flag != last_flaky_state:
last_flaky_state = flaky_dispenser.success_flag
print("== FLAKY DISPENSER FLIPPED SUCCESS STATE ==",
last_flaky_state)
if quiet_dispenser.success_flag and flaky_dispenser.success_flag:
rclpy_executor.spin_once(1)
break
rclpy_executor.spin_once(1)
time.sleep(0.2)
print("\n== DEBUG TASK REPORT ==")
print("Visited waypoints:", robot_cmd.visited_waypoints)
print("At least one incomplete:", at_least_one_incomplete)
print("Completed:", completed)
print()
assert len(robot_cmd.visited_waypoints) == 6
assert all([x in robot_cmd.visited_waypoints for x in [0, 5, 6, 7, 8, 10]])
assert robot_cmd.visited_waypoints[0] == 2
assert robot_cmd.visited_waypoints[5] == 4
assert robot_cmd.visited_waypoints[6] == 3
assert robot_cmd.visited_waypoints[7] == 1
assert robot_cmd.visited_waypoints[8] == 2
assert robot_cmd.visited_waypoints[10] == 1
assert at_least_one_incomplete
# Uncomment this to send a second request.
# TODO(CH3):
# But note! The TaskManager has to be fixed first to allow task pre-emption
# print("# SENDING NEW REQUEST ##########################################")
# request = adpt.type.CPPDeliveryMsg("test_delivery_two",
# dropoff_name,
# flaky_dispenser_name,
# pickup_name,
# quiet_dispenser_name)
# quiet_dispenser.reset()
# flaky_dispenser.reset()
# adapter.request_delivery(request)
# rclpy_executor.spin_once(1)
#
# for i in range(1000):
# if quiet_dispenser.success_flag != last_quiet_state:
# last_quiet_state = quiet_dispenser.success_flag
# print("== QUIET DISPENSER FLIPPED SUCCESS STATE ==",
# last_quiet_state)
#
# if flaky_dispenser.success_flag != last_flaky_state:
# last_flaky_state = flaky_dispenser.success_flag
# print("== FLAKY DISPENSER FLIPPED SUCCESS STATE ==",
# last_flaky_state)
#
# if quiet_dispenser.success_flag and flaky_dispenser.success_flag:
# rclpy_executor.spin_once(1)
# break
#
# rclpy_executor.spin_once(1)
# time.sleep(0.2)
#
# print("\n== DEBUG TASK REPORT ==")
# print("Visited waypoints:", robot_cmd.visited_waypoints)
# print("At least one incomplete:", at_least_one_incomplete)
# print("Completed:", completed)
# print()
task_node.destroy_node()
cmd_node.destroy_node()
quiet_dispenser.destroy_node()
flaky_dispenser.destroy_node()
rclpy_executor.shutdown()
rclpy.shutdown()
def main():
# INIT RCL ================================================================
rclpy.init()
try:
adpt.init_rclcpp()
except RuntimeError:
# Continue if it is already initialized
pass
# INIT GRAPH ==============================================================
test_graph = graph.Graph()
test_graph.add_waypoint(map_name, [0.0, -10.0]) # 0
test_graph.add_waypoint(map_name, [0.0, -5.0]) # 1
test_graph.add_waypoint(map_name, [5.0, -5.0]).set_holding_point(True) # 2
test_graph.add_waypoint(map_name, [-10.0, 0]) # 3
test_graph.add_waypoint(map_name, [-5.0, 0.0]) # 4
test_graph.add_waypoint(map_name, [0.0, 0.0]) # 5
test_graph.add_waypoint(map_name, [5.0, 0.0]) # 6
test_graph.add_waypoint(map_name, [10.0, 0.0]) # 7
test_graph.add_waypoint(map_name, [0.0, 5.0]) # 8
test_graph.add_waypoint(map_name, [5.0, 5.0]).set_holding_point(True) # 9
test_graph.add_waypoint(map_name, [0.0, 10.0]) # 10
assert test_graph.get_waypoint(2).holding_point
assert test_graph.get_waypoint(9).holding_point
assert not test_graph.get_waypoint(10).holding_point
test_graph_legend = \
"""
D : Dispenser
I : Ingestor
H : Holding Point
K : Dock
Numerals : Waypoints
---- : Lanes
"""
test_graph_vis = \
test_graph_legend + \
"""
10(I,K)
|
|
8------9(H)
| |
| |
3------4------5------6------7(D,K)
| |
| |
1------2(H)
|
|
0
"""
test_graph.add_bidir_lane(0, 1) # 0 1
test_graph.add_bidir_lane(1, 2) # 2 3
test_graph.add_bidir_lane(1, 5) # 4 5
test_graph.add_bidir_lane(2, 6) # 6 7
test_graph.add_bidir_lane(3, 4) # 8 9
test_graph.add_bidir_lane(4, 5) # 10 11
test_graph.add_bidir_lane(5, 6) # 12 13
test_graph.add_dock_lane(6, 7, "A") # 14 15
test_graph.add_bidir_lane(5, 8) # 16 17
test_graph.add_bidir_lane(6, 9) # 18 19
test_graph.add_bidir_lane(8, 9) # 20 21
test_graph.add_dock_lane(8, 10, "B") # 22 23
assert test_graph.num_lanes == 24
test_graph.add_key(pickup_name, 7)
test_graph.add_key(dropoff_name, 10)
assert len(test_graph.keys) == 2 and pickup_name in test_graph.keys \
and dropoff_name in test_graph.keys
# INIT FLEET ==============================================================
profile = traits.Profile(geometry.make_final_convex_circle(1.0))
robot_traits = traits.VehicleTraits(linear=traits.Limits(0.7, 0.3),
angular=traits.Limits(1.0, 0.45),
profile=profile)
# Manages delivery or loop requests
adapter = adpt.MockAdapter("TestDeliveryAdapter")
fleet = adapter.add_fleet(fleet_name, robot_traits, test_graph)
# Set up task request callback function
# we will only accept delivery task here
def task_request_cb(task_profile):
from rmf_task_msgs.msg import TaskType
if (task_profile.description.task_type == TaskType.TYPE_DELIVERY):
return True
else:
return False
fleet.accept_task_requests(task_request_cb)
# Set fleet battery profile
battery_sys = battery.BatterySystem.make(24.0, 40.0, 8.8)
mech_sys = battery.MechanicalSystem.make(70.0, 40.0, 0.22)
motion_sink = battery.SimpleMotionPowerSink(battery_sys, mech_sys)
ambient_power_sys = battery.PowerSystem.make(20.0)
ambient_sink = battery.SimpleDevicePowerSink(battery_sys,
ambient_power_sys)
tool_power_sys = battery.PowerSystem.make(10.0)
tool_sink = battery.SimpleDevicePowerSink(battery_sys, tool_power_sys)
fleet.set_recharge_threshold(0.2)
b_success = fleet.set_task_planner_params(battery_sys, motion_sink,
ambient_sink, tool_sink)
assert b_success, "set battery param failed"
cmd_node = Node("RobotCommandHandle")
# Test compute_plan_starts, which tries to place the robot on the navgraph
# Your robot MUST be near a waypoint or lane for this to work though!
starts = plan.compute_plan_starts(test_graph, map_name,
[[-10.0], [0.0], [0.0]], adapter.now())
assert [x.waypoint for x in starts] == [3], [x.waypoint for x in starts]
# Alternatively, if you DO know where your robot is, place it directly!
starts = [plan.Start(adapter.now(), 0, 0.0)]
# Lambda to insert an adapter
def updater_inserter(handle_obj, updater):
updater.update_battery_soc(1.0)
handle_obj.updater = updater
# Manages and executes robot commands
robot_cmd = MockRobotCommand(cmd_node, test_graph)
fleet.add_robot(robot_cmd, "T0", profile, starts,
partial(updater_inserter, robot_cmd))
# INIT DISPENSERS =========================================================
dispenser = MockDispenser(dispenser_name)
ingestor = MockIngestor(ingestor_name)
# INIT TASK SUMMARY OBSERVER ==============================================
# Note: this is used for assertation check on TaskSummary.Msg
observer = TaskSummaryObserver()
# FINAL PREP ==============================================================
rclpy_executor = SingleThreadedExecutor()
rclpy_executor.add_node(cmd_node)
rclpy_executor.add_node(dispenser)
rclpy_executor.add_node(ingestor)
rclpy_executor.add_node(observer)
# GO! =====================================================================
adapter.start()
print("\n")
print("# SENDING SINGLE DELIVERY REQUEST ################################")
print(test_graph_vis)
dispenser.reset()
ingestor.reset()
observer.reset()
observer.add_task(test_name)
task_desc = Type.CPPTaskDescriptionMsg()
# this is the time when the robot reaches the pick up point
task_desc.start_time_sec = int(time.time()) + 50
task_desc.delivery = Type.CPPDeliveryMsg(pickup_name, dispenser_name,
dropoff_name, ingestor_name)
task_profile = Type.CPPTaskProfileMsg()
task_profile.description = task_desc
task_profile.task_id = test_name
adapter.dispatch_task(task_profile)
rclpy_executor.spin_once(1)
for i in range(1000):
if observer.all_tasks_complete():
print("Tasks Complete.")
break
rclpy_executor.spin_once(1)
# time.sleep(0.2)
results = observer.count_successful_tasks()
print("\n== DEBUG TASK REPORT ==")
print("Visited waypoints:", robot_cmd.visited_waypoints)
print(f"Sucessful Tasks: {results[0]} / {results[1]}")
assert results[0] == results[1], "Not all tasks were completed."
error_msg = "Robot did not take the expected route"
assert robot_cmd.visited_waypoints == [
0, 0, 5, 5, 6, 6, 7, 6, 5, 5, 8, 8, 10
], error_msg
# check if unstable partcipant works
# this is helpful for to update the robot when it is
print("Update a custom itinerary to fleet adapter")
traj = schedule.make_trajectory(robot_traits, adapter.now(),
[[3, 0, 0], [1, 1, 0], [2, 1, 0]])
route = schedule.Route("test_map", traj)
participant = robot_cmd.updater.get_unstable_participant()
routeid = participant.last_route_id
participant.set_itinerary([route])
new_routeid = participant.last_route_id
print(f"Previous route id: {routeid} , new route id: {new_routeid}")
assert routeid != new_routeid
# TODO(YL) There's an issue with during shutdown of the adapter, occurs
# when set_itinerary() function above is used. Similarly with a non-mock
# adpater, will need to address this in near future
print("\n~ Shutting Down everything ~")
cmd_node.destroy_node()
observer.destroy_node()
dispenser.destroy_node()
ingestor.destroy_node()
robot_cmd.stop()
adapter.stop()
rclpy_executor.shutdown()
rclpy.shutdown()
def create_robot_command_handles(config, handle_data, dry_run=False):
robots = {}
for robot_name, robot_config in config['robots'].items():
# CONFIGURE MIR =======================================================
mir_config = robot_config['mir_config']
rmf_config = robot_config['rmf_config']
configuration = mir100_client.Configuration()
configuration.username = mir_config['base_url']
configuration.host = mir_config['user']
configuration.password = mir_config['password']
api_client = mir100_client.ApiClient(configuration)
api_client.default_headers['Accept-Language'] = 'en-US'
# CONFIGURE HANDLE ====================================================
robot = MiRCommandHandle(name=robot_name,
node=handle_data['node'],
rmf_graph=handle_data['graph'],
robot_state_update_frequency=rmf_config.get(
'robot_state_update_frequency', 1),
dry_run=dry_run)
robot.mir_api = mir100_client.DefaultApi(api_client)
robot.transforms = handle_data['transforms']
robot.rmf_map_name = rmf_config['start']['map_name']
if not dry_run:
with MiRRetryContext(robot):
_mir_status = robot.mir_api.status_get()
robot.mir_name = _mir_status.robot_name
robot.load_mir_missions()
robot.load_mir_positions()
else:
robot.mir_name = "DUMMY_ROBOT_FOR_DRY_RUN"
robots[robot.name] = robot
# OBTAIN PLAN STARTS ==================================================
start_config = rmf_config['start']
# If the plan start is configured, use it, otherwise, grab it
if (start_config.get('waypoint_index')
and start_config.get('orientation')):
starts = [
plan.Start(handle_data['adapter'].now(),
start_config.get('waypoint_index'),
start_config.get('orientation'))
]
else:
starts = plan.compute_plan_starts(
handle_data['graph'], start_config['map_name'],
robot.get_position(rmf=True, as_dimensions=True),
handle_data['adapter'].now())
assert starts, ("Robot %s can't be placed on the nav graph!" %
robot_name)
# Insert start data into robot
start = starts[0]
if start.lane.has_value: # If the robot is in a lane
robot.rmf_current_lane_index = start.lane.value
robot.rmf_current_waypoint_index = None
robot.rmf_target_waypoint_index = None
else: # Otherwise, the robot is on a waypoint
robot.rmf_current_lane_index = None
robot.rmf_current_waypoint_index = start.waypoint
robot.rmf_target_waypoint_index = None
print("MAP_NAME:", start_config['map_name'])
robot.rmf_map_name = start_config['map_name']
# INSERT UPDATER ======================================================
def updater_inserter(handle_obj, rmf_updater):
"""Insert a RobotUpdateHandle."""
handle_obj.rmf_updater = rmf_updater
handle_data['fleet_handle'].add_robot(robot, robot.name,
handle_data['profile'], starts,
partial(updater_inserter, robot))
handle_data['node'].get_logger().info(
f"successfully initialized robot {robot.name}"
f" (MiR name: {robot.mir_name})")
return robots
def __init__(self, name, config, node, graph, vehicle_traits, transforms,
map_name, initial_waypoint, initial_orientation,
charger_waypoint, update_frequency, adapter):
adpt.RobotCommandHandle.__init__(self)
self.name = name
self.config = config
self.node = node
self.graph = graph
self.vehicle_traits = vehicle_traits
self.transforms = transforms
self.map_name = map_name
self.initial_waypoint = initial_waypoint
self.initial_orientation = initial_orientation
# Get the index of the charger waypoint
waypoint = self.graph.find_waypoint(charger_waypoint)
assert waypoint, f"Charger waypoint {charger_waypoint} \
does not exist in the navigation graph"
self.charger_waypoint_index = waypoint.index
self.charger_is_set = False
self.update_frequency = update_frequency
self.update_handle = None # RobotUpdateHandle
self.battery_soc = 1.0
self.api = None
self.position = [] # (x,y,theta) in RMF coordinates (meters, radians)
self.initialized = False
self.state = RobotState.IDLE
self.dock_name = ""
self.adapter = adapter
self.requested_waypoints = [] # RMF Plan waypoints
self.remaining_waypoints = []
self.path_finished_callback = None
self.next_arrival_estimator = None
self.path_index = 0
self.docking_finished_callback = None
# RMF location trackers
self.last_known_lane_index = None
self.last_known_waypoint_index = None
# if robot is waiting at a waypoint. This is a Graph::Waypoint index
self.on_waypoint = None
# if robot is travelling on a lane. This is a Graph::Lane index
self.on_lane = None
self.target_waypoint = None # this is a Plan::Waypoint
# The graph index of the waypoint the robot is currently docking into
self.dock_waypoint_index = None
# Threading variables
self._lock = threading.Lock()
self._follow_path_thread = None
self._quit_path_event = threading.Event()
self._dock_thread = None
self._quit_dock_event = threading.Event()
# Establish connection with the robot
self.api = RobotAPI(
self.config['base_url'],
self.config['user'],
self.config['password'],
robot_name=name,
config=self.config,
vehicle_traits=vehicle_traits,
)
assert self.api.connected, "Unable to connect to Robot API server"
self.position = self.get_position() # RMF coordinates
assert len(
self.position) > 2, "Unable to get current location of the robot"
self.node.get_logger().info(
f"The robot is starting at: [{self.position[0]:.2f}, "
f"{self.position[1]:.2f}, {self.position[2]:.2f}]")
# Obtain StartSet for the robot
self.starts = []
time_now = self.adapter.now()
if (self.initial_waypoint is not None) and\
(self.initial_orientation is not None):
self.node.get_logger().info(
f"Using provided initial waypoint [{self.initial_waypoint}] "
f"and orientation [{self.initial_orientation:.2f}] to "
f"initialize starts for robot [{self.name}]")
# Get the waypoint index for initial_waypoint
initial_waypoint_index = self.graph.find_waypoint(
self.initial_waypoint).index
self.starts = [
plan.Start(time_now, initial_waypoint_index,
self.initial_orientation)
]
else:
self.node.get_logger().info(
f"Running compute_plan_starts for robot:{self.name}")
self.starts = plan.compute_plan_starts(self.graph, self.map_name,
self.position, time_now)
if self.starts is None or len(self.starts) == 0:
self.node.get_logger().error(
f"Unable to determine StartSet for {self.name}")
return
start = self.starts[0]
# Update tracking variables
if start.lane is not None: # If the robot is on a lane
self.last_known_lane_index = start.lane
self.on_lane = start.lane
self.last_known_waypoint_index = start.waypoint
else: # Otherwise, the robot is on a waypoint
self.last_known_waypoint_index = start.waypoint
self.on_waypoint = start.waypoint
self.state_update_timer = self.node.create_timer(
1.0 / self.update_frequency, self.update)
self.initialized = True