#!/usr/bin/env python3

import rospy
from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg


def talker():
    pub = rospy.Publisher('WeCookDispatch', TaskMsg, queue_size=10)
    rospy.init_node('wecook_chicken_pasta', anonymous=True)

    scene_msg = SceneMsg([ObjectMsg('wall0',
                                    'package://wecook_assets/data/furniture/wall.urdf',
                                    [0.75, 0.05, 0., 0., 0., 0., 1.]),
                          ObjectMsg('wall1',
                                    'package://wecook_assets/data/furniture/wall.urdf',
                                    [-0.85, 1.45, 0., 0., 0., 0.707, 0.707]),
                          ObjectMsg('counter0',
                                    'package://wecook_assets/data/furniture/kitchen_counter.urdf',
                                    [0.3, 0., 0., 0., 0., 0., 1.]),
                          ObjectMsg('counter1',
                                    'package://wecook_assets/data/furniture/kitchen_counter.urdf',
                                    [0., 1.0, 0., 0., 0., 0.707, 0.707]),
                          ObjectMsg('sink0',
                                    'package://wecook_assets/data/furniture/sink_counter.urdf',
                                    [-1.3, 1.05, 0., 0., 0., 0.707, 0.707]),
                          ObjectMsg('shelf0',
                                    'package://wecook_assets/data/furniture/bookcase.urdf',
                                    [0.3, -1.05, 0., 0., 0., 0., 1.]),
                          ObjectMsg('stove0',
                                    'package://wecook_assets/data/objects/stove.urdf',
                                    [-0.35, 0.95, 0.75, 0., 0., 0., 1.]),
                          ObjectMsg('pot0',
                                    'package://wecook_assets/data/objects/cooking_pot.urdf',
                                    [0.35, 1.1, 0.75, 0., 0., 0., 1.]),
                          ObjectMsg('skillet0',
                                    'package://wecook_assets/data/objects/skillet.urdf',
                                    [0.3, 0.7, 0.75, 0., 0., -0.707, .707]),
                          ObjectMsg('cutting_board0',
                                    'package://wecook_assets/data/objects/cutting_board.urdf',
                                    [0.3, -0.3, 0.75, 0., 0., 0., 1.]),         
                          ObjectMsg('knife0',
                                    'package://wecook_assets/data/objects/knife_big.urdf',
                                    [0.215, -0.55, 0.775, 0., 0., 0., 1.]),
                          ObjectMsg('plate0',
                                    'package://wecook_assets/data/objects/plate.urdf',
                                    [0.3, 0.075, 0.75, 0., 0., 0., 1.]),
                          ObjectMsg('bowl0',
                                    'package://wecook_assets/data/objects/bowl_green.urdf',
                                    [0.45, 0.375, 0.75, 0., 0., 0., 1.]),
                          ObjectMsg('bowl1',
                                    'package://wecook_assets/data/objects/bowl_green.urdf',
                                    [0.15, 0.375, 0.75, 0., 0., 0., 1.]),
                          ObjectMsg('oil0',
                                    'package://wecook_assets/data/objects/olive_oil.urdf',
                                    [0., 1.15, 0.75, 0., 0., 0.707, 0.707]),
                          ObjectMsg('salt0',
                                    'package://wecook_assets/data/objects/salt.urdf',
                                    [0., 1.0, 0.75, 0., 0., 0.707, 0.707]),
                          ObjectMsg('pepper0',
                                    'package://wecook_assets/data/objects/black_pepper.urdf',
                                    [0., 0.9, 0.75, 0., 0., 0.707, 0.707]),
                          ObjectMsg('chicken0',
                                    'package://wecook_assets/data/food/chicken.urdf',
                                    [0.3, 0.075, 0.757, 0., 0., 0., 1.]),
                          ObjectMsg('lime0',
                                    'package://wecook_assets/data/food/lime.urdf',
                                    [0.3, -0.3, 0.757, 0., 0., 0., 1.]),
                          ObjectMsg('pasta0',
                                    'package://wecook_assets/data/food/pasta.urdf',
                                    [0.45, 0.375, 0.757, 0., 0., 0., 1.])],
                         [ContainingMsg(['plate0', 'chicken0']),
                          ContainingMsg(['bowl0', 'pasta0'])])

    task_msg = TaskMsg(scene_msg,
                       [ActionMsg(['p1'], 'cut', ['plate0'], 'knife0', ['lime0'])],
                       [AgentMsg('p1', 'r', [0., 0., 0.75, 0., 0., 0., 0.])],
                       "",
                       "",
                       "follow",
                       "RRTConnect",
                       False)

    # sleeping 10 seconds to publish
    rospy.sleep(1)

    pub.publish(task_msg)


if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
        pass

[((8, 10, 8, 67), 'rospy.Publisher', 'rospy.Publisher', (), '', False, 'import rospy\n'), ((9, 4, 9, 59), 'rospy.init_node', 'rospy.init_node', (), '', False, 'import rospy\n'), ((84, 4, 84, 18), 'rospy.sleep', 'rospy.sleep', ({(84, 16, 84, 17): '(1)'}, {}), '(1)', False, 'import rospy\n'), ((11, 26, 13, 69), 'wecook.msg.ObjectMsg', 'ObjectMsg', ({(11, 36, 11, 43): '"""wall0"""', (12, 36, 12, 86): '"""package://wecook_assets/data/furniture/wall.urdf"""', (13, 36, 13, 68): '[0.75, 0.05, 0.0, 0.0, 0.0, 0.0, 1.0]'}, {}), "('wall0', 'package://wecook_assets/data/furniture/wall.urdf', [\n    0.75, 0.05, 0.0, 0.0, 0.0, 0.0, 1.0])", False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((14, 26, 16, 76), 'wecook.msg.ObjectMsg', 'ObjectMsg', ({(14, 36, 14, 43): '"""wall1"""', (15, 36, 15, 86): '"""package://wecook_assets/data/furniture/wall.urdf"""', (16, 36, 16, 75): '[-0.85, 1.45, 0.0, 0.0, 0.0, 0.707, 0.707]'}, {}), "('wall1', 'package://wecook_assets/data/furniture/wall.urdf', [-\n    0.85, 1.45, 0.0, 0.0, 0.0, 0.707, 0.707])", False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((17, 26, 19, 66), 'wecook.msg.ObjectMsg', 'ObjectMsg', ({(17, 36, 17, 46): '"""counter0"""', (18, 36, 18, 97): '"""package://wecook_assets/data/furniture/kitchen_counter.urdf"""', (19, 36, 19, 65): '[0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]'}, {}), "('counter0',\n    'package://wecook_assets/data/furniture/kitchen_counter.urdf', [0.3, \n    0.0, 0.0, 0.0, 0.0, 0.0, 1.0])", False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((20, 26, 22, 72), 'wecook.msg.ObjectMsg', 'ObjectMsg', ({(20, 36, 20, 46): '"""counter1"""', (21, 36, 21, 97): '"""package://wecook_assets/data/furniture/kitchen_counter.urdf"""', (22, 36, 22, 71): '[0.0, 1.0, 0.0, 0.0, 0.0, 0.707, 0.707]'}, {}), "('counter1',\n    'package://wecook_assets/data/furniture/kitchen_counter.urdf', [0.0, \n    1.0, 0.0, 0.0, 0.0, 0.707, 0.707])", False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((23, 26, 25, 75), 'wecook.msg.ObjectMsg', 'ObjectMsg', ({(23, 36, 23, 43): '"""sink0"""', (24, 36, 24, 94): '"""package://wecook_assets/data/furniture/sink_counter.urdf"""', (25, 36, 25, 74): '[-1.3, 1.05, 0.0, 0.0, 0.0, 0.707, 0.707]'}, {}), "('sink0',\n    'package://wecook_assets/data/furniture/sink_counter.urdf', [-1.3, 1.05,\n    0.0, 0.0, 0.0, 0.707, 0.707])", False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((26, 26, 28, 69), 'wecook.msg.ObjectMsg', 'ObjectMsg', ({(26, 36, 26, 44): '"""shelf0"""', (27, 36, 27, 90): '"""package://wecook_assets/data/furniture/bookcase.urdf"""', (28, 36, 28, 68): '[0.3, -1.05, 0.0, 0.0, 0.0, 0.0, 1.0]'}, {}), "('shelf0', 'package://wecook_assets/data/furniture/bookcase.urdf',\n    [0.3, -1.05, 0.0, 0.0, 0.0, 0.0, 1.0])", False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((29, 26, 31, 72), 'wecook.msg.ObjectMsg', 'ObjectMsg', ({(29, 36, 29, 44): '"""stove0"""', (30, 36, 30, 85): '"""package://wecook_assets/data/objects/stove.urdf"""', (31, 36, 31, 71): '[-0.35, 0.95, 0.75, 0.0, 0.0, 0.0, 1.0]'}, {}), "('stove0', 'package://wecook_assets/data/objects/stove.urdf', [-\n    0.35, 0.95, 0.75, 0.0, 0.0, 0.0, 1.0])", False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((32, 26, 34, 70), 'wecook.msg.ObjectMsg', 'ObjectMsg', ({(32, 36, 32, 42): '"""pot0"""', (33, 36, 33, 91): '"""package://wecook_assets/data/objects/cooking_pot.urdf"""', (34, 36, 34, 69): '[0.35, 1.1, 0.75, 0.0, 0.0, 0.0, 1.0]'}, {}), "('pot0', 'package://wecook_assets/data/objects/cooking_pot.urdf',\n    [0.35, 1.1, 0.75, 0.0, 0.0, 0.0, 1.0])", False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((35, 26, 37, 75), 'wecook.msg.ObjectMsg', 'ObjectMsg', ({(35, 36, 35, 46): '"""skillet0"""', (36, 36, 36, 87): '"""package://wecook_assets/data/objects/skillet.urdf"""', (37, 36, 37, 74): '[0.3, 0.7, 0.75, 0.0, 0.0, -0.707, 0.707]'}, {}), "('skillet0', 'package://wecook_assets/data/objects/skillet.urdf',\n    [0.3, 0.7, 0.75, 0.0, 0.0, -0.707, 0.707])", False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((38, 26, 40, 70), 'wecook.msg.ObjectMsg', 'ObjectMsg', ({(38, 36, 38, 52): '"""cutting_board0"""', (39, 36, 39, 93): '"""package://wecook_assets/data/objects/cutting_board.urdf"""', (40, 36, 40, 69): '[0.3, -0.3, 0.75, 0.0, 0.0, 0.0, 1.0]'}, {}), "('cutting_board0',\n    'package://wecook_assets/data/objects/cutting_board.urdf', [0.3, -0.3, \n    0.75, 0.0, 0.0, 0.0, 1.0])", False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((41, 26, 43, 74), 'wecook.msg.ObjectMsg', 'ObjectMsg', ({(41, 36, 41, 44): '"""knife0"""', (42, 36, 42, 89): '"""package://wecook_assets/data/objects/knife_big.urdf"""', (43, 36, 43, 73): '[0.215, -0.55, 0.775, 0.0, 0.0, 0.0, 1.0]'}, {}), "('knife0', 'package://wecook_assets/data/objects/knife_big.urdf',\n    [0.215, -0.55, 0.775, 0.0, 0.0, 0.0, 1.0])", False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((44, 26, 46, 71), 'wecook.msg.ObjectMsg', 'ObjectMsg', ({(44, 36, 44, 44): '"""plate0"""', (45, 36, 45, 85): '"""package://wecook_assets/data/objects/plate.urdf"""', (46, 36, 46, 70): '[0.3, 0.075, 0.75, 0.0, 0.0, 0.0, 1.0]'}, {}), "('plate0', 'package://wecook_assets/data/objects/plate.urdf', [0.3,\n    0.075, 0.75, 0.0, 0.0, 0.0, 1.0])", False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((47, 26, 49, 72), 'wecook.msg.ObjectMsg', 'ObjectMsg', ({(47, 36, 47, 43): '"""bowl0"""', (48, 36, 48, 90): '"""package://wecook_assets/data/objects/bowl_green.urdf"""', (49, 36, 49, 71): '[0.45, 0.375, 0.75, 0.0, 0.0, 0.0, 1.0]'}, {}), "('bowl0', 'package://wecook_assets/data/objects/bowl_green.urdf',\n    [0.45, 0.375, 0.75, 0.0, 0.0, 0.0, 1.0])", False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((50, 26, 52, 72), 'wecook.msg.ObjectMsg', 'ObjectMsg', ({(50, 36, 50, 43): '"""bowl1"""', (51, 36, 51, 90): '"""package://wecook_assets/data/objects/bowl_green.urdf"""', (52, 36, 52, 71): '[0.15, 0.375, 0.75, 0.0, 0.0, 0.0, 1.0]'}, {}), "('bowl1', 'package://wecook_assets/data/objects/bowl_green.urdf',\n    [0.15, 0.375, 0.75, 0.0, 0.0, 0.0, 1.0])", False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((53, 26, 55, 75), 'wecook.msg.ObjectMsg', 'ObjectMsg', ({(53, 36, 53, 42): '"""oil0"""', (54, 36, 54, 89): '"""package://wecook_assets/data/objects/olive_oil.urdf"""', (55, 36, 55, 74): '[0.0, 1.15, 0.75, 0.0, 0.0, 0.707, 0.707]'}, {}), "('oil0', 'package://wecook_assets/data/objects/olive_oil.urdf', [\n    0.0, 1.15, 0.75, 0.0, 0.0, 0.707, 0.707])", False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((56, 26, 58, 74), 'wecook.msg.ObjectMsg', 'ObjectMsg', ({(56, 36, 56, 43): '"""salt0"""', (57, 36, 57, 84): '"""package://wecook_assets/data/objects/salt.urdf"""', (58, 36, 58, 73): '[0.0, 1.0, 0.75, 0.0, 0.0, 0.707, 0.707]'}, {}), "('salt0', 'package://wecook_assets/data/objects/salt.urdf', [0.0, \n    1.0, 0.75, 0.0, 0.0, 0.707, 0.707])", False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((59, 26, 61, 74), 'wecook.msg.ObjectMsg', 'ObjectMsg', ({(59, 36, 59, 45): '"""pepper0"""', (60, 36, 60, 92): '"""package://wecook_assets/data/objects/black_pepper.urdf"""', (61, 36, 61, 73): '[0.0, 0.9, 0.75, 0.0, 0.0, 0.707, 0.707]'}, {}), "('pepper0',\n    'package://wecook_assets/data/objects/black_pepper.urdf', [0.0, 0.9, \n    0.75, 0.0, 0.0, 0.707, 0.707])", False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((62, 26, 64, 72), 'wecook.msg.ObjectMsg', 'ObjectMsg', ({(62, 36, 62, 46): '"""chicken0"""', (63, 36, 63, 84): '"""package://wecook_assets/data/food/chicken.urdf"""', (64, 36, 64, 71): '[0.3, 0.075, 0.757, 0.0, 0.0, 0.0, 1.0]'}, {}), "('chicken0', 'package://wecook_assets/data/food/chicken.urdf', [\n    0.3, 0.075, 0.757, 0.0, 0.0, 0.0, 1.0])", False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((65, 26, 67, 71), 'wecook.msg.ObjectMsg', 'ObjectMsg', ({(65, 36, 65, 43): '"""lime0"""', (66, 36, 66, 81): '"""package://wecook_assets/data/food/lime.urdf"""', (67, 36, 67, 70): '[0.3, -0.3, 0.757, 0.0, 0.0, 0.0, 1.0]'}, {}), "('lime0', 'package://wecook_assets/data/food/lime.urdf', [0.3, -\n    0.3, 0.757, 0.0, 0.0, 0.0, 1.0])", False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((68, 26, 70, 73), 'wecook.msg.ObjectMsg', 'ObjectMsg', ({(68, 36, 68, 44): '"""pasta0"""', (69, 36, 69, 82): '"""package://wecook_assets/data/food/pasta.urdf"""', (70, 36, 70, 72): '[0.45, 0.375, 0.757, 0.0, 0.0, 0.0, 1.0]'}, {}), "('pasta0', 'package://wecook_assets/data/food/pasta.urdf', [0.45, \n    0.375, 0.757, 0.0, 0.0, 0.0, 1.0])", False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((71, 26, 71, 63), 'wecook.msg.ContainingMsg', 'ContainingMsg', ({(71, 40, 71, 62): "['plate0', 'chicken0']"}, {}), "(['plate0', 'chicken0'])", False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((72, 26, 72, 60), 'wecook.msg.ContainingMsg', 'ContainingMsg', ({(72, 40, 72, 59): "['bowl0', 'pasta0']"}, {}), "(['bowl0', 'pasta0'])", False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((75, 24, 75, 81), 'wecook.msg.ActionMsg', 'ActionMsg', ({(75, 34, 75, 40): "['p1']", (75, 42, 75, 47): '"""cut"""', (75, 49, 75, 59): "['plate0']", (75, 61, 75, 69): '"""knife0"""', (75, 71, 75, 80): "['lime0']"}, {}), "(['p1'], 'cut', ['plate0'], 'knife0', ['lime0'])", False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((76, 24, 76, 75), 'wecook.msg.AgentMsg', 'AgentMsg', ({(76, 33, 76, 37): '"""p1"""', (76, 39, 76, 42): '"""r"""', (76, 44, 76, 74): '[0.0, 0.0, 0.75, 0.0, 0.0, 0.0, 0.0]'}, {}), "('p1', 'r', [0.0, 0.0, 0.75, 0.0, 0.0, 0.0, 0.0])", False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n')]