#!/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

[((151, 208), 'rospy.Publisher', 'rospy.Publisher', (['"""WeCookDispatch"""', 'TaskMsg'], {'queue_size': '(10)'}), "('WeCookDispatch', TaskMsg, queue_size=10)\n", (166, 208), False, 'import rospy\n'), ((213, 268), 'rospy.init_node', 'rospy.init_node', (['"""wecook_chicken_pasta"""'], {'anonymous': '(True)'}), "('wecook_chicken_pasta', anonymous=True)\n", (228, 268), False, 'import rospy\n'), ((5021, 5035), 'rospy.sleep', 'rospy.sleep', (['(1)'], {}), '(1)\n', (5032, 5035), False, 'import rospy\n'), ((296, 410), 'wecook.msg.ObjectMsg', 'ObjectMsg', (['"""wall0"""', '"""package://wecook_assets/data/furniture/wall.urdf"""', '[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])\n", (305, 410), False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((500, 619), 'wecook.msg.ObjectMsg', 'ObjectMsg', (['"""wall1"""', '"""package://wecook_assets/data/furniture/wall.urdf"""', '[-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])\n", (509, 619), False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((711, 841), 'wecook.msg.ObjectMsg', 'ObjectMsg', (['"""counter0"""', '"""package://wecook_assets/data/furniture/kitchen_counter.urdf"""', '[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])\n", (720, 841), False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((926, 1060), 'wecook.msg.ObjectMsg', 'ObjectMsg', (['"""counter1"""', '"""package://wecook_assets/data/furniture/kitchen_counter.urdf"""', '[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])\n", (935, 1060), False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((1147, 1276), 'wecook.msg.ObjectMsg', 'ObjectMsg', (['"""sink0"""', '"""package://wecook_assets/data/furniture/sink_counter.urdf"""', '[-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])\n", (1156, 1276), False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((1365, 1483), 'wecook.msg.ObjectMsg', 'ObjectMsg', (['"""shelf0"""', '"""package://wecook_assets/data/furniture/bookcase.urdf"""', '[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])\n", (1374, 1483), False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((1574, 1690), 'wecook.msg.ObjectMsg', 'ObjectMsg', (['"""stove0"""', '"""package://wecook_assets/data/objects/stove.urdf"""', '[-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])\n", (1583, 1690), False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((1781, 1898), 'wecook.msg.ObjectMsg', 'ObjectMsg', (['"""pot0"""', '"""package://wecook_assets/data/objects/cooking_pot.urdf"""', '[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])\n", (1790, 1898), False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((1990, 2111), 'wecook.msg.ObjectMsg', 'ObjectMsg', (['"""skillet0"""', '"""package://wecook_assets/data/objects/skillet.urdf"""', '[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])\n", (1999, 2111), False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((2204, 2338), 'wecook.msg.ObjectMsg', 'ObjectMsg', (['"""cutting_board0"""', '"""package://wecook_assets/data/objects/cutting_board.urdf"""', '[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])\n", (2213, 2338), False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((2434, 2555), 'wecook.msg.ObjectMsg', 'ObjectMsg', (['"""knife0"""', '"""package://wecook_assets/data/objects/knife_big.urdf"""', '[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])\n", (2443, 2555), False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((2647, 2761), 'wecook.msg.ObjectMsg', 'ObjectMsg', (['"""plate0"""', '"""package://wecook_assets/data/objects/plate.urdf"""', '[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])\n", (2656, 2761), False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((2853, 2972), 'wecook.msg.ObjectMsg', 'ObjectMsg', (['"""bowl0"""', '"""package://wecook_assets/data/objects/bowl_green.urdf"""', '[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])\n", (2862, 2972), False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((3064, 3183), 'wecook.msg.ObjectMsg', 'ObjectMsg', (['"""bowl1"""', '"""package://wecook_assets/data/objects/bowl_green.urdf"""', '[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])\n", (3073, 3183), False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((3275, 3395), 'wecook.msg.ObjectMsg', 'ObjectMsg', (['"""oil0"""', '"""package://wecook_assets/data/objects/olive_oil.urdf"""', '[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])\n", (3284, 3395), False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((3487, 3602), 'wecook.msg.ObjectMsg', 'ObjectMsg', (['"""salt0"""', '"""package://wecook_assets/data/objects/salt.urdf"""', '[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])\n", (3496, 3602), False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((3694, 3823), 'wecook.msg.ObjectMsg', 'ObjectMsg', (['"""pepper0"""', '"""package://wecook_assets/data/objects/black_pepper.urdf"""', '[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])\n", (3703, 3823), False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((3911, 4028), 'wecook.msg.ObjectMsg', 'ObjectMsg', (['"""chicken0"""', '"""package://wecook_assets/data/food/chicken.urdf"""', '[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])\n", (3920, 4028), False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((4119, 4229), 'wecook.msg.ObjectMsg', 'ObjectMsg', (['"""lime0"""', '"""package://wecook_assets/data/food/lime.urdf"""', '[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])\n", (4128, 4229), False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((4320, 4434), 'wecook.msg.ObjectMsg', 'ObjectMsg', (['"""pasta0"""', '"""package://wecook_assets/data/food/pasta.urdf"""', '[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])\n", (4329, 4434), False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((4526, 4563), 'wecook.msg.ContainingMsg', 'ContainingMsg', (["['plate0', 'chicken0']"], {}), "(['plate0', 'chicken0'])\n", (4539, 4563), False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((4591, 4625), 'wecook.msg.ContainingMsg', 'ContainingMsg', (["['bowl0', 'pasta0']"], {}), "(['bowl0', 'pasta0'])\n", (4604, 4625), False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((4687, 4744), 'wecook.msg.ActionMsg', 'ActionMsg', (["['p1']", '"""cut"""', "['plate0']", '"""knife0"""', "['lime0']"], {}), "(['p1'], 'cut', ['plate0'], 'knife0', ['lime0'])\n", (4696, 4744), False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n'), ((4771, 4828), 'wecook.msg.AgentMsg', 'AgentMsg', (['"""p1"""', '"""r"""', '[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])\n", (4779, 4828), False, 'from wecook.msg import ActionMsg, TaskMsg, SceneMsg, ObjectMsg, ContainingMsg, AgentMsg\n')]