def main():
rclpy.init()
adpt.init_rclcpp()
profile = traits.Profile(geometry.make_final_convex_circle(1.0))
robot_traits = traits.VehicleTraits(linear=traits.Limits(2.5, 1.0),
angular=traits.Limits(2.5, 1.0),
profile=profile)
print("Test Run Easy Traffic Light")
path1, path2 = init_graph()
adapter = adpt.Adapter.make("TestTrafficLightAdapter")
# Add robot 1
mock_tl = MockTrafficLightHandle("dummybot1")
adapter.add_easy_traffic_light(mock_tl.traffic_light_cb, "dummybot_fleet",
mock_tl.name, robot_traits,
mock_tl.pause_cb, mock_tl.resume_cb)
# Add robot 2
mock_tl2 = MockTrafficLightHandle("dummybot2")
adapter.add_easy_traffic_light(mock_tl2.traffic_light_cb, "dummybot_fleet",
mock_tl2.name, robot_traits,
mock_tl2.pause_cb, mock_tl2.resume_cb)
adapter.start()
# this is crucial to wait for traffic light to start
time.sleep(1)
# inform rmf that the robot is following these path
mock_tl.pub_follow_path(path1)
mock_tl2.pub_follow_path(path2)
# Move robot along the designated path.
def control_robot(mod, path):
for cp in range(len(path) - 1):
if not mod.move_robot(cp):
print("Error! Robot Failed to move")
break
time.sleep(0.2)
th1 = Thread(target=control_robot, args=(mock_tl, path1))
th2 = Thread(target=control_robot, args=(mock_tl2, path2))
th1.start()
th2.start()
th1.join()
th2.join()
print(f"Sequence of visited waypoints: \n{visited_waypoints}")
assert visited_waypoints == [
("dummybot2", 3), ("dummybot2", 4), ("dummybot1", 2), ("dummybot2", 6),
("dummybot1", 3), ("dummybot1", 4), ("dummybot1", 5)
], "Sequence of visited waypoints is incorrect."
print("Done Traffic Light Tutorial!")
rclpy.shutdown()
def main():
print("Starting Simple Dispatcher Node")
adpt.init_rclcpp()
dispatcher = Nodes.DispatcherNode.make_node("sample_dispatcher_node")
th1 = Thread(target=submit_task_thread, args=(dispatcher,))
th1.start()
while True:
adpt.spin_some_rclcpp(dispatcher.node())
time.sleep(0.2)
print("Exiting")
def main():
adpt.init_rclcpp()
print("Creating Sceduler and Blockade Node")
blockade_node = nodes.make_blockade(adpt.NodeOptions())
schedule_node = nodes.make_schedule(adpt.NodeOptions())
print("spinning rclcpp")
while True:
try:
adpt.spin_some_rclcpp(blockade_node)
adpt.spin_some_rclcpp(schedule_node)
time.sleep(0.1)
except RuntimeError as e:
# TODO: needa fix runtime error during deregistration
print('Error on spining: ', e)
print("Exiting")
def main(argv=sys.argv):
# Init rclpy and adapter
rclpy.init(args=argv)
adpt.init_rclcpp()
print(f"Starting fleet adapter...")
# ROS 2 node for the command handle
node = rclpy.node.Node('robot_command_handle')
node.declare_parameter("config_file", rclpy.Parameter.Type.STRING)
node.declare_parameter("nav_graph_file", rclpy.Parameter.Type.STRING)
arg_use_sim_time = node.get_parameter(
'use_sim_time').get_parameter_value().bool_value
assert arg_use_sim_time is not None
arg_config_path = node.get_parameter(
'config_file').get_parameter_value().string_value
assert arg_config_path is not None
arg_nav_graph_path = node.get_parameter(
'nav_graph_file').get_parameter_value().string_value
assert arg_nav_graph_path is not None
# Load config and nav graph yamls
with open(arg_config_path, "r") as f:
config_yaml = yaml.safe_load(f)
adapter, fleet_handle, robots = initialize_fleet(config_yaml,
arg_nav_graph_path, node,
arg_use_sim_time)
# Create executor for the command handle node
rclpy_executor = rclpy.executors.SingleThreadedExecutor()
rclpy_executor.add_node(node)
# Start the fleet adapter
rclpy_executor.spin()
# Shutdown
node.destroy_node()
rclpy_executor.shutdown()
rclpy.shutdown()
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(args, delivery_condition=None, mock=False):
config_path = args.config_path
mock = args.mock
dry_run = args.dry_run # For testing
if dry_run:
mock = True
with open(config_path, "r") as f:
config = yaml.safe_load(f)
sanitise_dict(config, inplace=True, recursive=True)
print("\n== Initialising MiR Robot Command Handles with Config ==")
pprint(config)
print()
# INIT RCL ================================================================
rclpy.init()
adpt.init_rclcpp()
# INIT GRAPH ==============================================================
nav_graph = create_nav_graph(config)
# INIT FLEET ==============================================================
adapter, fleet, fleet_name, profile = create_fleet(config,
nav_graph,
mock=mock)
# INIT TRANSFORMS =========================================================
rmf_coordinates = config['reference_coordinates']['rmf']
mir_coordinates = config['reference_coordinates']['mir']
transforms = compute_transforms(rmf_coordinates, mir_coordinates)
# INIT ROBOT HANDLES ======================================================
cmd_node = rclpy.node.Node(config['node_names']['robot_command_handle'])
handle_data = {
'fleet_handle': fleet,
'fleet_name': fleet_name,
'adapter': adapter,
'node': cmd_node,
'graph': nav_graph,
'profile': profile,
'transforms': transforms
}
robots = create_robot_command_handles(config, handle_data, dry_run=dry_run)
# CREATE NODE EXECUTOR ====================================================
rclpy_executor = rclpy.executors.SingleThreadedExecutor()
rclpy_executor.add_node(cmd_node)
# INIT FLEET STATE PUB ====================================================
if config['rmf_fleet']['publish_fleet_state']:
fleet_state_node = rclpy.node.Node(
config['node_names']['fleet_state_publisher'])
fleet_state_pub = fleet_state_node.create_publisher(
FleetState, config['rmf_fleet']['fleet_state_topic'], 1)
rclpy_executor.add_node(fleet_state_node)
def create_fleet_state_pub_fn(fleet_state_pub, fleet_name, robots):
def f():
fleet_state = FleetState()
fleet_state.name = fleet_name
for robot in robots.values():
fleet_state.robots.append(robot.robot_state)
fleet_state_pub.publish(fleet_state)
return f
fleet_state_timer = fleet_state_node.create_timer(
config['rmf_fleet']['fleet_state_publish_frequency'],
create_fleet_state_pub_fn(fleet_state_pub, fleet_name, robots))
# GO! =====================================================================
adapter.start()
rclpy_executor.spin()
# CLEANUP =================================================================
cmd_node.destroy_node()
if config['rmf_fleet']['publish_fleet_state']:
fleet_state_node.destroy_timer(fleet_state_timer)
fleet_state_node.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()
