def setup_environment():
psi = smi_.get_planning_scene_interface()
rospy.sleep(1.0)
#smi_.clear_objects("arm_7_link")
smi_.clear_objects("arm_left_7_link")
### Add a floor
smi_.add_ground()
### Add table
pose = PoseStamped()
pose.header.frame_id = "/base_footprint"
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = -0.9
pose.pose.position.y = 0
pose.pose.position.z = 0.39
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
psi.add_box("bookcase", pose, size=(0.5, 1.5, 0.78))
rospy.sleep(1.0)
if finished_before_timeout:
state = place_action_client.get_state()
print "Action finished: %s" % state
# Prints out the result of executing the action
return state # State after waiting for CobPlaceAction
if __name__ == '__main__':
try:
# Initializes a rospy node so that the SimpleActionClient can
# publish and subscribe over ROS.
rospy.init_node('CobPickAction_client_py')
setup_environment()
psi = smi_.get_planning_scene_interface()
rospy.sleep(1.0)
filename = roslib.packages.get_pkg_dir(
'cob_pick_place_action') + "/files/meshes/yellowsaltcube.stl"
pose1 = gen_pose(pos=[-0.7, 0.0, 0.85],
euler=[
random.uniform(-pi, pi),
random.uniform(-pi, pi),
random.uniform(-pi, pi)
])
psi.add_mesh("cube1", pose1, filename)
rospy.sleep(1.0)
pose2 = gen_pose(pos=[-0.7, 0.2, 0.85],
euler=[