#!/usr/bin/env python

import rospy
import roslib
from tf.transformations import *
from simple_moveit_interface import *


def gen_pose(frame_id="/odom_combined", pos=[0,0,0], euler=[0,0,0]):
	pose = PoseStamped()
	pose.header.frame_id = frame_id
	pose.header.stamp = rospy.Time.now()
	pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = pos
	pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w = quaternion_from_euler(*euler)
	return pose

if __name__ == '__main__':
	rospy.init_node('spawn_grasp_object')
	while rospy.get_time() == 0.0: pass

	psi = get_planning_scene_interface()
	rospy.sleep(1.0)

	### ADDING OBJECT
	object_name = "pringles"
	pose = gen_pose(frame_id="base_footprint", pos=[-0.7, 0.5, 1.05], euler=[0, 0, 0])
	filename = roslib.packages.get_pkg_dir('cob_pick_place_action')+'/files/meshes/'+object_name+'.stl'
	#psi.add_mesh(object_name, pose, filename)	#assimp error
	psi.add_box(object_name, pose, size=(0.1, 0.1, 0.1))

	rospy.sleep(1.0)


f9612c9b87f668b9d4b7a7f4053cbfb1ed6aa6e4
<mask token>

<mask token>


def gen_pose(frame_id='/odom_combined', pos=[0, 0, 0], euler=[0, 0, 0]):
    pose = PoseStamped()
    pose.header.frame_id = frame_id
    pose.header.stamp = rospy.Time.now()
    pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = pos
    (pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.
        orientation.z, pose.pose.orientation.w) = quaternion_from_euler(*euler)
    return pose


<mask token>

<mask token>


def gen_pose(frame_id='/odom_combined', pos=[0, 0, 0], euler=[0, 0, 0]):
    pose = PoseStamped()
    pose.header.frame_id = frame_id
    pose.header.stamp = rospy.Time.now()
    pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = pos
    (pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.
        orientation.z, pose.pose.orientation.w) = quaternion_from_euler(*euler)
    return pose


if __name__ == '__main__':
    rospy.init_node('spawn_grasp_object')
    while rospy.get_time() == 0.0:
        pass
    psi = get_planning_scene_interface()
    rospy.sleep(1.0)
    object_name = 'pringles'
    pose = gen_pose(frame_id='base_footprint', pos=[-0.7, 0.5, 1.05], euler
        =[0, 0, 0])
    filename = roslib.packages.get_pkg_dir('cob_pick_place_action'
        ) + '/files/meshes/' + object_name + '.stl'
    psi.add_box(object_name, pose, size=(0.1, 0.1, 0.1))
    rospy.sleep(1.0)

import rospy
import roslib
from tf.transformations import *
from simple_moveit_interface import *


def gen_pose(frame_id='/odom_combined', pos=[0, 0, 0], euler=[0, 0, 0]):
    pose = PoseStamped()
    pose.header.frame_id = frame_id
    pose.header.stamp = rospy.Time.now()
    pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = pos
    (pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.
        orientation.z, pose.pose.orientation.w) = quaternion_from_euler(*euler)
    return pose


if __name__ == '__main__':
    rospy.init_node('spawn_grasp_object')
    while rospy.get_time() == 0.0:
        pass
    psi = get_planning_scene_interface()
    rospy.sleep(1.0)
    object_name = 'pringles'
    pose = gen_pose(frame_id='base_footprint', pos=[-0.7, 0.5, 1.05], euler
        =[0, 0, 0])
    filename = roslib.packages.get_pkg_dir('cob_pick_place_action'
        ) + '/files/meshes/' + object_name + '.stl'
    psi.add_box(object_name, pose, size=(0.1, 0.1, 0.1))
    rospy.sleep(1.0)

#!/usr/bin/env python

import rospy
import roslib
from tf.transformations import *
from simple_moveit_interface import *


def gen_pose(frame_id="/odom_combined", pos=[0,0,0], euler=[0,0,0]):
	pose = PoseStamped()
	pose.header.frame_id = frame_id
	pose.header.stamp = rospy.Time.now()
	pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = pos
	pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w = quaternion_from_euler(*euler)
	return pose

if __name__ == '__main__':
	rospy.init_node('spawn_grasp_object')
	while rospy.get_time() == 0.0: pass

	psi = get_planning_scene_interface()
	rospy.sleep(1.0)

	### ADDING OBJECT
	object_name = "pringles"
	pose = gen_pose(frame_id="base_footprint", pos=[-0.7, 0.5, 1.05], euler=[0, 0, 0])
	filename = roslib.packages.get_pkg_dir('cob_pick_place_action')+'/files/meshes/'+object_name+'.stl'
	#psi.add_mesh(object_name, pose, filename)	#assimp error
	psi.add_box(object_name, pose, size=(0.1, 0.1, 0.1))

	rospy.sleep(1.0)

