def create_fleet(config, nav_graph, mock):
"""Create RMF Adapter and FleetUpdateHandle."""
profile = traits.Profile(
geometry.make_final_convex_circle(
config['rmf_fleet']['profile']['radius']))
robot_traits = traits.VehicleTraits(
linear=traits.Limits(*config['rmf_fleet']['limits']['linear']),
angular=traits.Limits(*config['rmf_fleet']['limits']['angular']),
profile=profile)
# RMF_CORE Fleet Adapter: Manages delivery or loop requests
if mock:
adapter = adpt.MockAdapter(config['node_names']['rmf_fleet_adapter'])
else:
adapter = adpt.Adapter.make(config['node_names']['rmf_fleet_adapter'])
assert adapter, ("Adapter could not be init! "
"Ensure a ROS2 scheduler node is running")
fleet_name = config['rmf_fleet']['name']
fleet = adapter.add_fleet(fleet_name, robot_traits, nav_graph)
if delivery_condition is None:
# Naively accept all delivery requests
fleet.accept_delivery_requests(lambda x: True)
else:
fleet.accept_delivery_requests(delivery_condition)
return adapter, fleet, fleet_name, profile
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():
# 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 initialize_fleet(config_yaml, nav_graph_path, node, use_sim_time):
# Profile and traits
fleet_config = config_yaml['rmf_fleet']
profile = traits.Profile(
geometry.make_final_convex_circle(
fleet_config['profile']['footprint']),
geometry.make_final_convex_circle(fleet_config['profile']['vicinity']))
vehicle_traits = traits.VehicleTraits(
linear=traits.Limits(*fleet_config['limits']['linear']),
angular=traits.Limits(*fleet_config['limits']['angular']),
profile=profile)
vehicle_traits.differential.reversible = fleet_config['reversible']
# Battery system
voltage = fleet_config['battery_system']['voltage']
capacity = fleet_config['battery_system']['capacity']
charging_current = fleet_config['battery_system']['charging_current']
battery_sys = battery.BatterySystem.make(voltage, capacity,
charging_current)
# Mechanical system
mass = fleet_config['mechanical_system']['mass']
moment = fleet_config['mechanical_system']['moment_of_inertia']
friction = fleet_config['mechanical_system']['friction_coefficient']
mech_sys = battery.MechanicalSystem.make(mass, moment, friction)
# Power systems
ambient_power_sys = battery.PowerSystem.make(
fleet_config['ambient_system']['power'])
tool_power_sys = battery.PowerSystem.make(
fleet_config['tool_system']['power'])
# Power sinks
motion_sink = battery.SimpleMotionPowerSink(battery_sys, mech_sys)
ambient_sink = battery.SimpleDevicePowerSink(battery_sys,
ambient_power_sys)
tool_sink = battery.SimpleDevicePowerSink(battery_sys, tool_power_sys)
nav_graph = graph.parse_graph(nav_graph_path, vehicle_traits)
# Adapter
fleet_name = fleet_config['name']
adapter = adpt.Adapter.make(f'{fleet_name}_fleet_adapter')
if use_sim_time:
adapter.node.use_sim_time()
assert adapter, ("Unable to initialize fleet adapter. Please ensure "
"RMF Schedule Node is running")
adapter.start()
time.sleep(1.0)
fleet_handle = adapter.add_fleet(fleet_name, vehicle_traits, nav_graph)
if not fleet_config['publish_fleet_state']:
fleet_handle.fleet_state_publish_period(None)
# Account for battery drain
drain_battery = fleet_config['account_for_battery_drain']
recharge_threshold = fleet_config['recharge_threshold']
recharge_soc = fleet_config['recharge_soc']
finishing_request = fleet_config['task_capabilities']['finishing_request']
node.get_logger().info(f"Finishing request: [{finishing_request}]")
# Set task planner params
ok = fleet_handle.set_task_planner_params(battery_sys, motion_sink,
ambient_sink, tool_sink,
recharge_threshold, recharge_soc,
drain_battery, finishing_request)
assert ok, ("Unable to set task planner params")
task_capabilities = []
if fleet_config['task_capabilities']['loop']:
node.get_logger().info(
f"Fleet [{fleet_name}] is configured to perform Loop tasks")
task_capabilities.append(TaskType.TYPE_LOOP)
if fleet_config['task_capabilities']['delivery']:
node.get_logger().info(
f"Fleet [{fleet_name}] is configured to perform Delivery tasks")
task_capabilities.append(TaskType.TYPE_DELIVERY)
if fleet_config['task_capabilities']['clean']:
node.get_logger().info(
f"Fleet [{fleet_name}] is configured to perform Clean tasks")
task_capabilities.append(TaskType.TYPE_CLEAN)
# Callable for validating requests that this fleet can accommodate
def _task_request_check(task_capabilities, msg: TaskProfile):
if msg.description.task_type in task_capabilities:
return True
else:
return False
fleet_handle.accept_task_requests(
partial(_task_request_check, task_capabilities))
# Transforms
rmf_coordinates = config_yaml['reference_coordinates']['rmf']
robot_coordinates = config_yaml['reference_coordinates']['robot']
transforms = {
'rmf_to_robot': nudged.estimate(rmf_coordinates, robot_coordinates),
'robot_to_rmf': nudged.estimate(robot_coordinates, rmf_coordinates)
}
transforms['orientation_offset'] = \
transforms['rmf_to_robot'].get_rotation()
mse = nudged.estimate_error(transforms['rmf_to_robot'], rmf_coordinates,
robot_coordinates)
print(f"Coordinate transformation error: {mse}")
print("RMF to Robot transform:")
print(f" rotation:{transforms['rmf_to_robot'].get_rotation()}")
print(f" scale:{transforms['rmf_to_robot'].get_scale()}")
print(f" trans:{transforms['rmf_to_robot'].get_translation()}")
print("Robot to RMF transform:")
print(f" rotation:{transforms['robot_to_rmf'].get_rotation()}")
print(f" scale:{transforms['robot_to_rmf'].get_scale()}")
print(f" trans:{transforms['robot_to_rmf'].get_translation()}")
def _updater_inserter(cmd_handle, update_handle):
"""Insert a RobotUpdateHandle."""
cmd_handle.update_handle = update_handle
# Initialize robots for this fleet
robots = {}
for robot_name, robot_config in config_yaml['robots'].items():
node.get_logger().info(f"Initializing robot:{robot_name}")
rmf_config = robot_config['rmf_config']
robot_config = robot_config['robot_config']
initial_waypoint = rmf_config['start']['waypoint']
initial_orientation = rmf_config['start']['orientation']
robot = RobotCommandHandle(
name=robot_name,
config=robot_config,
node=node,
graph=nav_graph,
vehicle_traits=vehicle_traits,
transforms=transforms,
map_name=rmf_config['start']['map_name'],
initial_waypoint=initial_waypoint,
initial_orientation=initial_orientation,
charger_waypoint=rmf_config['charger']['waypoint'],
update_frequency=rmf_config.get('robot_state_update_frequency', 1),
adapter=adapter)
if robot.initialized:
robots[robot_name] = robot
# Add robot to fleet
fleet_handle.add_robot(robot, robot_name, profile, robot.starts,
partial(_updater_inserter, robot))
node.get_logger().info(
f"Successfully added new robot:{robot_name}")
else:
node.get_logger().error(f"Failed to initialize robot:{robot_name}")
return adapter, fleet_handle, robots
assert (
profile.vicinity.characteristic_length == circle.characteristic_length)
# Check that vicinity is not overwritten
assert (profile_with_vicinity.footprint.characteristic_length ==
final_circle.characteristic_length)
assert (profile_with_vicinity.vicinity.characteristic_length ==
circle.characteristic_length)
# VEHICLETRAITS ===============================================================
vel_limits = traits.Limits(1, 1)
ang_limits = traits.Limits(2, 2)
# Check instantiation
valid_traits = traits.VehicleTraits(vel_limits, ang_limits, profile)
invalid_traits = traits.VehicleTraits(traits.Limits(1, -1), ang_limits,
profile, differential)
def test_vehicletraits():
# Check validity
assert valid_traits.valid
assert not invalid_traits.valid # Differential passed is invalid
# Check getters
assert valid_traits.linear.nominal_velocity == 1.0
assert valid_traits.linear.nominal_acceleration == 1.0
assert valid_traits.rotational.nominal_velocity == 2.0
assert valid_traits.rotational.nominal_acceleration == 2.0
assert valid_traits.profile.footprint.characteristic_length == 5.0