def test_format_place_task(self):
print ("Testing method: " + str(self._testMethodName))
tm = TaskManager()
result = ActionList()
t = Task(t_type=1, source=2, destination=4, container=2, t_object=4)
result.action_list = tm.format_place_task(t, result.action_list)
action_key = TaskProtocol.look_up_value(TaskProtocol.task_action_dict, "PLACE")
self.assertEqual(len(result.action_list), 1)
for item in result.action_list:
self.assertEqual(item.action, action_key)
self.assertEqual(item.action_str, "PLACE")
self.assertEqual(item.type, t.type)
self.assertEqual(item.type_str, t.type_str)
self.assertEqual(item.source, t.source)
self.assertEqual(item.source_str, t.source_str)
self.assertEqual(item.destination, t.destination)
self.assertEqual(item.destination_str, t.destination_str)
self.assertEqual(item.object, t.object)
self.assertEqual(item.object_str, t.object_str)
self.assertEqual(item.container, t.container)
self.assertEqual(item.container_str, t.container_str)
def __init__(self):
# Save params
self.verbose = rospy.get_param('~verbose')
self.yaml_path = rospy.get_param('~yaml_path')
self.mux_occupied = False
# Loggity log
rospy.loginfo("Initializing Task Manager Node...")
rospy.loginfo(" === PARAMETERS ==== ")
rospy.loginfo("Navigation YAML: %s", self.yaml_path)
rospy.loginfo("Verbose logging: %r", self.verbose)
rospy.loginfo("===================")
# for rotating table test
# rospy.Subscriber("/suii_refbox_client/conveyor_belt_status", TriggeredConveyorBeltStatus, self.conveyor_callback)
# fro receiving ROS tasks from the refbox client
rospy.Subscriber("/suii_refbox_client/task_info", TaskInfo,
self.task_callback)
rospy.Subscriber("/suii_refbox_client/stop_pressed", Empty,
self.stop_pressed_callback)
# The things
self.task_manager = TaskManager(holding_capacity=3,
yaml_path=self.yaml_path,
verbose=self.verbose)
self.task_list_pub = rospy.Publisher('/suii_task_executor/input',
SuiiTaskList,
queue_size=10,
latch=True)
self.task_replan_sub = rospy.Subscriber('/suii_task_executor/replan',
SuiiTaskList,
self.replan_callback)
# self.conveyor_belt = TriggeredConveyorBeltStatus()
rospy.loginfo("Task Manager node initialized!")
def test_format_set_of_tasks(self):
print ("Testing method: " + str(self._testMethodName))
tm = TaskManager()
tl = TaskList()
t1 = Task(t_type=1, source=2, destination=4, t_object=4)
tl.add_task(t1)
t2 = Task(t_type=1, source=3, destination=3, t_object=2)
tl.add_task(t2)
t3 = Task(t_type=1, source=2, destination=4, t_object=3)
tl.add_task(t3)
result = ActionList()
result.action_list = tm.format_set_of_tasks(tl, result.action_list)
def test_format_pick_from_robot(self):
print ("Testing method: " + str(self._testMethodName))
tm = TaskManager()
result = ActionList()
t = Task(t_type=1, source=2, destination=4, container=2, t_object=4)
result.action_list = tm.format_pick_from_robot(t, result.action_list)
action_key = TaskProtocol.look_up_value(TaskProtocol.task_action_dict, "PICK_FROM_ROBOT")
self.assertEqual(len(result.action_list), 1)
for item in result.action_list:
self.assertEqual(item.action, action_key)
self.assertEqual(item.action_str, "PICK_FROM_ROBOT")
self.assertEqual(item.object, t.object)
self.assertEqual(item.object_str, t.object_str)
def test_format_drive(self):
print ("Testing method: " + str(self._testMethodName))
tm = TaskManager()
result = ActionList()
dest_key = TaskProtocol.look_up_value(TaskProtocol.location_dict, "Workstation 1")
action_key = TaskProtocol.look_up_value(TaskProtocol.task_action_dict, "DRIVE")
result.action_list = tm.format_drive(dest_key, result.action_list)
self.assertEqual(len(result.action_list), 1)
for item in result.action_list:
self.assertEqual(item.action, action_key)
self.assertEqual(item.action_str, "DRIVE")
self.assertEqual(item.destination, dest_key)
self.assertEqual(item.destination_str, "Workstation 1")
def test_optimize_list(self):
print ("Testing method: " + str(self._testMethodName))
tm = TaskManager()
tl = TaskList()
t1 = Task(t_type=2, source=2, destination=1, t_object=4)
tl.add_task(t1)
t2 = Task(t_type=2, source=3, destination=3, t_object=2)
tl.add_task(t2)
t3 = Task(t_type=2, source=2, destination=4, t_object=3)
tl.add_task(t3)
t4 = Task(t_type=2, source=5, destination=4, t_object=3)
tl.add_task(t4)
result = ActionList()
result.action_list = tm.optimize_list(tl, result.action_list)
print(result)
def test_format_pick (self):
print ("Testing method: " + str(self._testMethodName))
tm = TaskManager()
tl = TaskList()
t1 = Task(t_type=1, source=2, destination=4, container=2, t_object=4)
tl.add_task(t1)
t2 = Task(t_type=1, source=2, destination=3, container=5, t_object=2)
tl.add_task(t2)
t3 = Task(t_type=1, source=3, destination=4, container=2, t_object=3)
tl.add_task(t3)
result = ActionList()
result.action_list = tm.format_pick(tl, result.action_list)
# If reached here, format_pick_task passed the test
# So only assert len
self.assertEquals(len(result.action_list), 3)
def test_constructor(self):
print ("Testing method: " + str(self._testMethodName))
tm = TaskManager()
self.assertEqual(tm.holding_capacity, tm.MAX_HOLDING_CAPACITY)
tm = TaskManager(holding_capacity=5)
self.assertEqual(tm.holding_capacity, 5)
class TaskMangerHandler():
def __init__(self):
# Save params
self.verbose = rospy.get_param('~verbose')
self.yaml_path = rospy.get_param('~yaml_path')
self.mux_occupied = False
# Loggity log
rospy.loginfo("Initializing Task Manager Node...")
rospy.loginfo(" === PARAMETERS ==== ")
rospy.loginfo("Navigation YAML: %s", self.yaml_path)
rospy.loginfo("Verbose logging: %r", self.verbose)
rospy.loginfo("===================")
# for rotating table test
# rospy.Subscriber("/suii_refbox_client/conveyor_belt_status", TriggeredConveyorBeltStatus, self.conveyor_callback)
# fro receiving ROS tasks from the refbox client
rospy.Subscriber("/suii_refbox_client/task_info", TaskInfo,
self.task_callback)
rospy.Subscriber("/suii_refbox_client/stop_pressed", Empty,
self.stop_pressed_callback)
# The things
self.task_manager = TaskManager(holding_capacity=3,
yaml_path=self.yaml_path,
verbose=self.verbose)
self.task_list_pub = rospy.Publisher('/suii_task_executor/input',
SuiiTaskList,
queue_size=10,
latch=True)
self.task_replan_sub = rospy.Subscriber('/suii_task_executor/replan',
SuiiTaskList,
self.replan_callback)
# self.conveyor_belt = TriggeredConveyorBeltStatus()
rospy.loginfo("Task Manager node initialized!")
def stop_pressed_callback(self, msg):
rospy.loginfo("Stop pressed on refbox. Ready for new task list.")
self.mux_occupied = False
def task_callback(self, msg):
if not self.mux_occupied:
rospy.loginfo("Tasks received. Converting to TaskObject...")
self.mux_occupied = True
self.task_manager.clear() # Clear shit
self.task_cb_convert_data(msg) # Convert ros msg to TaskList()
self.task_cb_optimize() # Send said TaskList() to TM
self.send_to_mux(
self.task_manager.output_list) # Send TM's ActionList() to Mux
def replan_callback(self, msg):
rospy.loginfo("Received replan data. Converting...")
# Update data for TM
self.task_manager.replan_list = MuxConverter.ros_to_action_list(msg)
self.task_manager.error_index = msg.error_index
# Replan
if (self.task_manager.replan()):
self.send_to_mux(self.task_manager.output_list)
else:
rospy.logfatal("TM cannot handle this error.")
def task_cb_filter_data(self, converted_list):
rospy.loginfo(
"Skipping tasks with 'Shelf' in source or destination...")
result = converted_list.make_duplicate()
for task in converted_list.task_list:
if ('Shelf' in task.destination_str or 'Shelf' in task.source_str):
result.task_list.remove(task)
return result
def task_cb_convert_data(self, msg):
converted_list = RefBoxConverter.ros_msg_to_task_list_object(msg)
if converted_list is None:
rospy.logerr("Cannot convert ROS msg to task list")
else:
# rospy.loginfo("Converted to task list:")
# rospy.loginfo(converted_list)
# rospy.loginfo("##################")
converted_list = self.task_cb_filter_data(converted_list)
self.task_manager.initialize_list(converted_list)
# rospy.loginfo("##################")
print(
'----------------------------------------------------------------------------'
)
print(self.task_manager.task_list)
print(
'----------------------------------------------------------------------------'
)
def task_cb_optimize(self):
optimize_ok = self.task_manager.optimize_list()
if (optimize_ok):
rospy.loginfo("Finished optimizing")
else:
rospy.logerr("Failed to optimize")
rospy.loginfo("Final commands list:")
print(
'----------------------------------------------------------------------------'
)
print(self.task_manager.output_list)
print(
'----------------------------------------------------------------------------'
)
def send_to_mux(self, result):
if (len(result.task_list) != 0):
rospy.loginfo("Publishing to mux...")
# Converting action list to mux with status DISPATCHED
mux_msg = MuxConverter.action_list_to_ros(
result.task_list, int(TaskStatus.DISPATCHED))
self.task_list_pub.publish(mux_msg)
else:
rospy.logwarn("Empty task list so didn't send to mux")
def conveyor_callback(self, msg):
self.conveyor_belt = msg