def create_nav_graph(config):
"""Construct RMF navigation graph from config file."""
nav_graph = graph.Graph()
waypoint_index_offset = 0 # Offset to account for prior maps
for map_name, map_config in config.get('rmf_maps', {}).items():
# Add waypoints
for waypoint in map_config.get('waypoints', []):
nav_graph.add_waypoint(map_name, waypoint)
# Set holding points
for holding_index in map_config.get('holding_points', []):
waypoint = nav_graph.get_waypoint(waypoint_index_offset +
holding_index)
waypoint.set_holding_point(True)
# Set passthrough points
for passthrough_index in map_config.get('passthrough_points', []):
waypoint = nav_graph.get_waypoint(waypoint_index_offset +
passthrough_index)
waypoint.set_passthrough_point(True)
# Set parking spots
for parking_index in map_config.get('parking_spots', []):
waypoint = nav_graph.get_waypoint(waypoint_index_offset +
parking_index)
waypoint.set_parking_spot(True)
# Add lanes
for lane in map_config.get('lanes', []):
lane = [graph.lane.Node(waypoint_index_offset + x) for x in lane]
nav_graph.add_lane(*lane)
# Add bidirectional lanes
for bidir_lane in map_config.get('bidir_lanes', []):
bidir_lane = [waypoint_index_offset + x for x in bidir_lane]
nav_graph.add_bidir_lane(*bidir_lane)
# Add dock lanes
for dock_name, dock in map_config.get('dock_lanes', {}).items():
if not dock.get('lane'):
continue
dock_lane = [waypoint_index_offset + x for x in dock['lane']]
dock_seconds = dock.get('seconds', 10)
nav_graph.add_dock_lane(*dock_lane, str(dock_name), dock_seconds)
# Add keys
for key_name, waypoint_index in map_config.get('keys', {}).items():
nav_graph.add_key(str(key_name),
waypoint_index_offset + waypoint_index)
waypoint_index_offset = nav_graph.num_waypoints
return nav_graph
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()
node_two = lane.Node(2,
event_ptr,
acceptable_orientation_constraint_ptr)
# Once you've instantiated your nodes, you can manipulate their members
# as you wish
def test_node():
node_one.orientation_constraint.clone()
node_one.orientation_constraint.apply(three_d, two_d)
# GRAPH =======================================================================
# Finally!
map_name = "rawr_map"
rawr_graph = graph.Graph()
def test_graph():
# Test waypoints
rawr_graph.add_waypoint(map_name, [0.0, -10.0]) # 0
rawr_graph.add_waypoint(map_name, [0.0, -5.0]) # 1
rawr_graph.add_waypoint(map_name, [5.0, -5.0]) # 2
rawr_graph.add_waypoint(map_name, [-10.0, 0]) # 3
rawr_graph.add_waypoint(map_name, [-5.0, 0.0]) # 4
rawr_graph.add_waypoint(map_name, [0.0, 0.0]) # 5
rawr_graph.add_waypoint(map_name, [5.0, 0.0]) # 6
rawr_graph.add_waypoint(map_name, [10.0, 0.0]) # 7
rawr_graph.add_waypoint(map_name, [0.0, 5.0]) \
.set_holding_point(True) # 8
rawr_graph.add_waypoint(map_name, [5.0, 5.0]) \