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

    scene_msg = SceneMsg([ObjectMsg('table0',
                                    'package://wecook_assets/data/furniture/table.urdf',
                                    [0.5, 0.0, 0.0, 0., 0., 0., 1.]),
                          ObjectMsg('pot0',
                                    'package://wecook_assets/data/objects/pot_s.urdf',
                                    [0.55, -0.25, 0.73, 0., 0., 0., 1.]),
                          ObjectMsg('pan',
                                    'package://wecook_assets/data/objects/pan.urdf',
                                    [0.4, -0.45, 0.73, 0., 0., 0., 1.]),
                          ObjectMsg('food_item0',
                                    'package://wecook_assets/data/food/food_item1.urdf',
                                    [0.3, 0.4, 0.72, 0., 0., 0., 1.]),
                          ObjectMsg('roller0',
                                    'package://wecook_assets/data/objects/roller.urdf',
                                    [0.3, 0.5, 0.83, 0., 0., 0., 1.])],
                         # ObjectMsg('cooker0',
                         #           'package://wecook_assets/data/objects/cooker.urdf',
                         #           [-0.55, 0, 0.45, 0., 0., 0., 1])],
                         [])

    task_msg = TaskMsg(scene_msg, [ActionMsg(['p1'], 'roll', [], 'roller0', ['food_item0'])])

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

    pub.publish(task_msg)
Esempio n. 2
0
def talker():
    pub = rospy.Publisher('WeCookDispatch', TaskMsg, queue_size=10)
    rospy.init_node('wecook_fruit_jelly', anonymous=True)

    scene_msg = SceneMsg(
        [
            ObjectMsg('table0',
                      'package://wecook_assets/data/furniture/table.urdf',
                      [0.5, 0.0, 0.0, 0., 0., 0., 1.]),
            ObjectMsg('pot0', 'package://wecook_assets/data/objects/pot.urdf',
                      [0.35, 0.2, 0.73, 0., 0., 0., 1.]),
            ObjectMsg('spoon1',
                      'package://wecook_assets/data/objects/spoon.urdf',
                      [0.15, -0.55, 0.75, 0, 0., 0, 1.]),
            ObjectMsg('food_item0',
                      'package://wecook_assets/data/food/food_item1.urdf',
                      [0.3, 0.4, 0.75, 0., 0., 0., 1.]),
            ObjectMsg('bowl0',
                      'package://wecook_assets/data/objects/bowl.urdf',
                      [0.3, 0.4, 0.73, 0., 0., 0., 1])
        ],
        # ObjectMsg('cooker0',
        #           'package://wecook_assets/data/objects/cooker.urdf',
        #           [-0.55, 0, 0.45, 0., 0., 0., 1])],
        [ContainingMsg(['bowl0', 'food_item0'])])

    task_msg = TaskMsg(scene_msg, [
        ActionMsg(['p1'], 'transfer', ['bowl0', 'pot0'], 'bowl0',
                  ['food_item0']),
        ActionMsg(['p2'], 'stir', ['pot0'], 'spoon1', ['food_item0'])
    ])

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

    pub.publish(task_msg)
Esempio n. 3
0
def talker():
    pub = rospy.Publisher('WeCookDispatch', TaskMsg, queue_size=10)
    rospy.init_node('wecook_fruit_jelly', anonymous=True)

    scene_msg = SceneMsg(
        [
            ObjectMsg('table0',
                      'package://wecook_assets/data/furniture/table.urdf',
                      [0.5, 0.0, 0.0, 0., 0., 0., 1.]),
            ObjectMsg('plate1',
                      'package://wecook_assets/data/objects/plate.urdf',
                      [0.3, -0.0, 0.73, 0., 0., 0., 1.]),
            ObjectMsg('knife0',
                      'package://wecook_assets/data/objects/knife.urdf',
                      [0.15, 0.55, 0.78, 0., 0, 0., 1.]),
            ObjectMsg('knife1',
                      'package://wecook_assets/data/objects/knife.urdf',
                      [0.15, -0.55, 0.78, 0., 0, 0., 1.]),
            ObjectMsg('spoon1',
                      'package://wecook_assets/data/objects/spoon.urdf',
                      [0.30, -0.50, 0.78, 0, 0., 0, 1.]),
            ObjectMsg(
                'chopping_board0',
                'package://wecook_assets/data/objects/chopping_board.urdf',
                [0.5, -0.35, 0.73, 0, 0, 0., 1.]),
            ObjectMsg('food_item0',
                      'package://wecook_assets/data/food/food_item2.urdf',
                      [0.4, -0.4, 0.75, 0., 0., 0., 1.]),
            ObjectMsg('food_item1',
                      'package://wecook_assets/data/food/food_item2.urdf',
                      [0.45, -0.45, 0.75, 0., 0., 0., 1.]),
            ObjectMsg('food_item3',
                      'package://wecook_assets/data/food/food_item0.urdf',
                      [0.3, -0., 0.75, 0., 0., 0., 1.]),
            ObjectMsg('spoon0',
                      'package://wecook_assets/data/objects/spoon.urdf',
                      [0.30, 0.50, 0.78, 0, 0., 0, 1.]),
            ObjectMsg('knifeHolder0',
                      'package://wecook_assets/data/objects/holder.urdf',
                      [0.25, 0.55, 0.73, 0., 0., 0., 1]),
            ObjectMsg('spoonHolder0',
                      'package://wecook_assets/data/objects/holder.urdf',
                      [0.25, 0.50, 0.73, 0., 0., 0., 1]),
            ObjectMsg('knifeHolder1',
                      'package://wecook_assets/data/objects/holder.urdf',
                      [0.25, -0.55, 0.73, 0., 0., 0., 1]),
            ObjectMsg('spoonHolder1',
                      'package://wecook_assets/data/objects/holder.urdf',
                      [0.25, -0.50, 0.73, 0., 0., 0., 1])
        ],
        # ObjectMsg('cooker0',
        #           'package://wecook_assets/data/objects/cooker.urdf',
        #           [-0.55, 0, 0.45, 0., 0., 0., 1])],
        [
            ContainingMsg(['chopping_board0', 'food_item0']),
            ContainingMsg(['chopping_board0', 'food_item1']),
            ContainingMsg(['plate1', 'food_item3'])
        ])

    task_msg = TaskMsg(
        scene_msg,
        [ActionMsg(['p2', 'p1'], 'handover', ['air'], 'hand', ['food_item0'])])

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

    pub.publish(task_msg)
Esempio n. 4
0
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', ['cutting_board0'], 'knife0', ['lime0'])], [
            AgentMsg('p1', 'r', [0., 0., 0.75, 0., 0., 0., 1.]),
            AgentMsg('p2', 'h', [-0.5, 0.25, 1.00, 0., 0., 0.707, 0.707])
        ], "", "", "follow", "RRTConnect", True)

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

    pub.publish(task_msg)
Esempio n. 5
0
def talker():
    pub = rospy.Publisher('WeCookDispatch', TaskMsg, queue_size=10)
    rospy.init_node('wecook_ham_sandwich', anonymous=True)

    scene_msg = SceneMsg([
        ObjectMsg('wall0', 'package://wecook_assets/data/furniture/wall.urdf',
                  [1.3, 0.15, 0., 0., 0., 0., 1.]),
        ObjectMsg('wall1', 'package://wecook_assets/data/furniture/wall.urdf',
                  [-0.3, 1.55, 0., 0., 0., 0.707, 0.707]),
        ObjectMsg(
            'counter0',
            'package://wecook_assets/data/furniture/kitchen_counter.urdf',
            [0.55, -0.3, 0., 0., 0., -0.707, 0.707]),
        ObjectMsg(
            'counter1',
            'package://wecook_assets/data/furniture/kitchen_counter.urdf',
            [0.55, 1.1, 0., 0., 0., 0.707, 0.707]),
        ObjectMsg('sink0',
                  'package://wecook_assets/data/furniture/sink_counter.urdf',
                  [-0.75, 1.125, 0., 0., 0., 0.707, 0.707]),
        ObjectMsg('shelf0',
                  'package://wecook_assets/data/furniture/bookcase.urdf',
                  [-0.75, -0.45, 0., 0., 0., 0.707, 0.707]),
        ObjectMsg('stove0', 'package://wecook_assets/data/objects/stove.urdf',
                  [0.9, -0.3, 0.75, 0., 0., 1., 0.]),
        ObjectMsg('toaster0',
                  'package://wecook_assets/data/objects/toaster.urdf',
                  [0.7, 1.25, 0.775, 0., 0., 0., 1.]),
        ObjectMsg('skillet0',
                  'package://wecook_assets/data/objects/skillet.urdf',
                  [0.9, 0.9, 0.75, 0., 0., 0., 1.]),
        ObjectMsg('cutting_board0',
                  'package://wecook_assets/data/objects/cutting_board.urdf',
                  [0.3, 0.95, 0.75, 0., 0., -0.707, 0.707]),
        ObjectMsg('knife0',
                  'package://wecook_assets/data/objects/knife_big.urdf',
                  [0.6, 0.85, 0.775, 0., 0., 0.707, 0.707]),
        ObjectMsg('brush0', 'package://wecook_assets/data/objects/brush.urdf',
                  [0.525, 0.95, 0.75, 0.707, 0., 0., 0.707]),
        ObjectMsg('plate0', 'package://wecook_assets/data/objects/plate.urdf',
                  [0.05, -0.3, 0.75, 0., 0., 0., 1.]),
        ObjectMsg('plate1', 'package://wecook_assets/data/objects/plate.urdf',
                  [0.3, -0.15, 0.75, 0., 0., 0., 1.]),
        ObjectMsg('plate2', 'package://wecook_assets/data/objects/plate.urdf',
                  [0.3, -0.45, 0.75, 0., 0., 0., 1.]),
        ObjectMsg('bowl0',
                  'package://wecook_assets/data/objects/bowl_green.urdf',
                  [0.3, 1.25, 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('bread0',
                  'package://wecook_assets/data/food/bread_slice.urdf',
                  [0.05, -0.25, 0.765, 0., 0., 0., 1.]),
        ObjectMsg('bread1',
                  'package://wecook_assets/data/food/bread_slice.urdf',
                  [0.05, -0.35, 0.765, 0., 0., 0., 1.]),
        ObjectMsg('cheese0',
                  'package://wecook_assets/data/food/cheese_slice.urdf',
                  [0.25, -0.15, 0.765, 0., 0., 0., 1.]),
        ObjectMsg('cheese1',
                  'package://wecook_assets/data/food/cheese_slice.urdf',
                  [0.35, -0.15, 0.765, 0., 0., 0., 1.]),
        ObjectMsg('ham0', 'package://wecook_assets/data/food/ham_slice.urdf',
                  [0.35, -0.45, 0.765, 0., 0., 0., 1.]),
        ObjectMsg('ham1', 'package://wecook_assets/data/food/ham_slice.urdf',
                  [0.25, -0.45, 0.765, 0., 0., 0., 1.]),
        ObjectMsg('butter0', 'package://wecook_assets/data/food/butter.urdf',
                  [0.3, 1.25, 0.765, 0., 0., 0., 1.])
    ], [
        ContainingMsg(['plate0', 'bread0']),
        ContainingMsg(['plate1', 'cheese0'])
    ])

    task_msg = TaskMsg(
        scene_msg,
        [ActionMsg(['p1'], 'stir', ['bowl0'], 'brush0', ['butter0'])],
        [AgentMsg('p1', 'r', [0., 0.85, 0.75, 0., 0., 0., 0.])], "", "",
        "follow", "RRTConnect", True)

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

    pub.publish(task_msg)
Esempio n. 6
0
def talker():
    pub = rospy.Publisher('WeCookDispatch', TaskMsg, queue_size=10)
    rospy.init_node('wecook_fruit_jelly', anonymous=True)

    scene_msg = SceneMsg([
        ObjectMsg('table0',
                  'package://wecook_assets/data/furniture/table.urdf',
                  [0.5, 0.0, 0.0, 0., 0., 0., 1.]),
        ObjectMsg('oil0', 'package://wecook_assets/data/objects/oil.urdf',
                  [0.2, -0.95, 0.87, 0., 0., 0., 1.]),
        ObjectMsg('plant0', 'package://wecook_assets/data/objects/plant.urdf',
                  [0.65, -0.75, 0.73, 0., 0., 0., 1.]),
        ObjectMsg('plate1', 'package://wecook_assets/data/objects/plate.urdf',
                  [0.2, -0.15, 0.73, 0., 0., 0., 1.]),
        ObjectMsg('plate0', 'package://wecook_assets/data/objects/plate.urdf',
                  [0.2, 0.2, 0.73, 0., 0., 0., 1.]),
        ObjectMsg('bowl0', 'package://wecook_assets/data/objects/tbowl.urdf',
                  [0.4, 0.0, 0.73, 0., 0., 0., 1.]),
        ObjectMsg('shelf0',
                  'package://wecook_assets/data/furniture/shelf.urdf',
                  [0., -1.10, 0.65, 0., 0., 0., 1.]),
        ObjectMsg('knife1', 'package://wecook_assets/data/objects/knife.urdf',
                  [0.18, -0.55, 0.78, 0., 0, 0., 1.]),
        ObjectMsg('knifeHolder1',
                  'package://wecook_assets/data/objects/holder.urdf',
                  [0.25, -0.55, 0.73, 0., 0., 0., 1]),
        ObjectMsg('knife0', 'package://wecook_assets/data/objects/knife.urdf',
                  [0.18, 0.65, 0.78, 0., 0, 0., 1.]),
        ObjectMsg('knifeHolder0',
                  'package://wecook_assets/data/objects/holder.urdf',
                  [0.25, 0.65, 0.73, 0., 0., 0., 1]),
        ObjectMsg('cooker0',
                  'package://wecook_assets/data/objects/cooker.urdf',
                  [-0.8, 0., 0.86, 0., 0., 0., 1.]),
        ObjectMsg('spoon1', 'package://wecook_assets/data/objects/spoon.urdf',
                  [0.3, -0.4, 0.78, 0, -0.7073, 0, 0.7073883]),
        ObjectMsg('food_item1',
                  'package://wecook_assets/data/food/food_item0.urdf',
                  [0.2, -0.15, 0.75, 0., 0., 0., 1.]),
        ObjectMsg('food_item0',
                  'package://wecook_assets/data/food/food_item1.urdf',
                  [0.2, 0.2, 0.75, 0., 0., 0., 1.]),
        ObjectMsg('spoonHolder1',
                  'package://wecook_assets/data/objects/holder.urdf',
                  [0.25, -0.4, 0.73, 0., 0., 0., 1]),
        ObjectMsg('spoon0', 'package://wecook_assets/data/objects/spoon.urdf',
                  [0.3, 0.45, 0.78, 0, -0.7073, 0, 0.7073883]),
        ObjectMsg('spoonHolder0',
                  'package://wecook_assets/data/objects/holder.urdf',
                  [0.25, 0.45, 0.73, 0., 0., 0., 1])
    ], [
        ContainingMsg(['plate1', 'food_item1']),
        ContainingMsg(['plate0', 'food_item0'])
    ])

    task_msg = TaskMsg(scene_msg, [
        ActionMsg(['p2'], 'cut', ['plate1'], 'knife1', ['food_item1']),
        ActionMsg(['p1', 'p2'], 'holding_plate0_transfer', ['plate0', 'bowl0'],
                  'spoon1', ['food_item0']),
        ActionMsg(['p2'], 'stir', ['bowl0'], 'spoon1', ['food_item0']),
        ActionMsg(['p1', 'p2'], 'holding_plate1_transfer', ['plate1', 'bowl0'],
                  'spoon1', ['food_item1']),
        ActionMsg(['p2'], 'stir', ['bowl0'], 'spoon1',
                  ['food_item0', 'food_item1']),
        ActionMsg(['p1'], 'stir', ['bowl0'], 'spoon0',
                  ['food_item0', 'food_item1'])
    ], [
        AgentMsg('p1', 'r', [-0.2, 0.15, 0.7, 0., 0., 0.7071068, 0.7071068]),
        AgentMsg('p2', 'r', [-0.2, -0.2, 0.7, 0., 0., 0.7071068, 0.7071068])
    ], "", "", "follow", "RRTConnect", True)

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

    pub.publish(task_msg)
Esempio n. 7
0
def talker():
    pub = rospy.Publisher('WeCookDispatch', TaskMsg, queue_size=10)
    rospy.init_node('wecook_mass_verde_2777', anonymous=True)

    scene_msg = SceneMsg([ObjectMsg('table0',
                                    'package://wecook_assets/data/furniture/table.urdf',
                                    [0.5, 0.0, 0.0, 0., 0., 0., 1.]),
                          ObjectMsg('oil0',
                                    'package://wecook_assets/data/objects/oil.urdf',
                                    [0.2, -0.95, 0.87, 0., 0., 0., 1.]),
                          ObjectMsg('shelf0',
                                    'package://wecook_assets/data/furniture/shelf.urdf',
                                    [0., -1.10, 0.65, 0., 0., 0., 1.]),
                          ObjectMsg('plant0',
                                    'package://wecook_assets/data/objects/plant.urdf',
                                    [0.65, -0.75, 0.73, 0., 0., 0., 1.]),
                          ObjectMsg('bowl0',
                                    'package://wecook_assets/data/objects/bowl.urdf',
                                    [0.5, -0.45, 0.73, 0., 0., 0., 1.]),
                          ObjectMsg('bowl1',
                                    'package://wecook_assets/data/objects/bowl.urdf',
                                    [0.6, -0.6, 0.73, 0., 0., 0., 1.]),
                          ObjectMsg('knife0',
                                    'package://wecook_assets/data/objects/knife.urdf',
                                    [0.18, 0.65, 0.78, 0., 0, 0., 1.]),
                          ObjectMsg('knifeHolder0',
                                    'package://wecook_assets/data/objects/holder.urdf',
                                    [0.25, 0.65, 0.73, 0., 0., 0., 1]),
                          ObjectMsg('cooker0',
                                    'package://wecook_assets/data/objects/cooker.urdf',
                                    [-0.8, 0., 0.86, 0., 0., 0., 1.]),
                          ObjectMsg('knife1',
                                    'package://wecook_assets/data/objects/knife.urdf',
                                    [0.18, -0.55, 0.78, 0., 0, 0., 1.]),
                          ObjectMsg('knifeHolder1',
                                    'package://wecook_assets/data/objects/holder.urdf',
                                    [0.25, -0.55, 0.73, 0., 0., 0., 1]),
                          ObjectMsg('spoon1',
                                    'package://wecook_assets/data/objects/spoon.urdf',
                                    [0.3, -0.4, 0.78, 0, -0.7073, 0, 0.7073883]),
                          ObjectMsg('spoonHolder1',
                                    'package://wecook_assets/data/objects/holder.urdf',
                                    [0.25, -0.4, 0.73, 0., 0., 0., 1]),
                          ObjectMsg('chopping_board0',
                                    'package://wecook_assets/data/objects/chopping_board.urdf',
                                    [0.3, -0., 0.73, 0, 0, 0., 1.]),
                          ObjectMsg('food_item3',
                                    'package://wecook_assets/data/food/food_item0.urdf',
                                    [0.3, -0., 0.75, 0., 0., 0., 1.]),
                          ObjectMsg('spoon0',
                                    'package://wecook_assets/data/objects/spoon.urdf',
                                    [0.3, 0.45, 0.78, 0, -0.7073, 0, 0.7073883]),
                          ObjectMsg('spoonHolder0',
                                    'package://wecook_assets/data/objects/holder.urdf',
                                    [0.25, 0.45, 0.73, 0., 0., 0., 1])],
                         # ObjectMsg('cooker0',
                         #           'package://wecook_assets/data/objects/cooker.urdf',
                         #           [-0.55, 0, 0.45, 0., 0., 0., 1])],
                         [ContainingMsg(['chopping_board0', 'food_item3'])])

    task_msg = TaskMsg(scene_msg, [ActionMsg(['p2', 'p1'], 'holding_chopping_board0_cut',
                                             ['chopping_board0'], 'knife0', ['food_item3'])])

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

    pub.publish(task_msg)
Esempio n. 8
0
def talker():
    pub = rospy.Publisher('WeCookDispatch', TaskMsg, queue_size=10)
    rospy.init_node('wecook_feeding', anonymous=True)

    scene_msg = SceneMsg([
        ObjectMsg('table0',
                  'package://wecook_assets/data/furniture/table.urdf',
                  [0.5, 0.0, 0.0, 0., 0., 0., 1.]),
        ObjectMsg('oil0', 'package://wecook_assets/data/objects/oil.urdf',
                  [0.2, -0.95, 0.87, 0., 0., 0., 1.]),
        ObjectMsg('plant0', 'package://wecook_assets/data/objects/plant.urdf',
                  [0.65, -0.75, 0.73, 0., 0., 0., 1.]),
        ObjectMsg('bowl0', 'package://wecook_assets/data/objects/bowl.urdf',
                  [0.4, -0.1, 0.73, 0., 0., 0., 1.]),
        ObjectMsg('shelf0',
                  'package://wecook_assets/data/furniture/shelf.urdf',
                  [0., -1.10, 0.65, 0., 0., 0., 1.]),
        ObjectMsg('cooker0',
                  'package://wecook_assets/data/objects/cooker.urdf',
                  [-0.8, 0., 0.86, 0., 0., 0., 1.]),
        ObjectMsg('food_item0',
                  'package://wecook_assets/data/food/food_item1.urdf',
                  [0.4, -0.1, 0.75, 0., 0., 0., 1.]),
        ObjectMsg('food_item1',
                  'package://wecook_assets/data/food/food_item1.urdf',
                  [0.4, -0.1, 0.75, 0., 0., 0., 1.]),
        ObjectMsg('food_item2',
                  'package://wecook_assets/data/food/food_item1.urdf',
                  [0.4, -0.1, 0.75, 0., 0., 0., 1.]),
        ObjectMsg('food_item3',
                  'package://wecook_assets/data/food/food_item1.urdf',
                  [0.4, -0.1, 0.75, 0., 0., 0., 1.]),
        ObjectMsg('food_item4',
                  'package://wecook_assets/data/food/food_item1.urdf',
                  [0.4, -0.1, 0.75, 0., 0., 0., 1.]),
        ObjectMsg('spoon0', 'package://wecook_assets/data/objects/spoon.urdf',
                  [0.3, -0.5, 0.78, 0, -0.7073, 0, 0.7073883]),
        ObjectMsg('spoonHolder0',
                  'package://wecook_assets/data/objects/holder.urdf',
                  [0.25, -0.5, 0.73, 0., 0., 0., 1]),
        ObjectMsg('mouth0', 'package://wecook_assets/data/human/mouth.urdf',
                  [0., 0.0, 1.1, 0., 0., 0., 1.])
    ], [
        ContainingMsg(['bowl0', 'food_item0']),
        ContainingMsg(['bowl0', 'food_item1']),
        ContainingMsg(['bowl0', 'food_item2']),
        ContainingMsg(['bowl0', 'food_item3']),
        ContainingMsg(['bowl0', 'food_item4'])
    ])

    task_msg = TaskMsg(scene_msg, [
        ActionMsg(['p1'], 'feeding', ['bowl0', 'mouth0'], 'spoon0',
                  ['food_item0']),
        ActionMsg(['p1'], 'feeding', ['bowl0', 'mouth0'], 'spoon0',
                  ['food_item1']),
        ActionMsg(['p1'], 'feeding', ['bowl0', 'mouth0'], 'spoon0',
                  ['food_item2']),
        ActionMsg(['p1'], 'feeding', ['bowl0', 'mouth0'], 'spoon0',
                  ['food_item3']),
        ActionMsg(['p1'], 'feeding', ['bowl0', 'mouth0'], 'spoon0',
                  ['food_item4'])
    ], [AgentMsg('p1', 'r', [-0., -0.3, 0.75, 0., 0., 0.7071068, 0.7071068])],
                       "", "", "follow", "RRTConnect", True)

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

    pub.publish(task_msg)