load_scene_example.py

Go to the documentation of this file.

#!/usr/bin/env python
import roslib; roslib.load_manifest('cob_moveit_interface')
import rospy

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('load_scene')
        while rospy.get_time() == 0.0: pass
        
        psi = get_planning_scene_interface()
        rospy.sleep(1.0)
        
        ### Add a floor
        add_ground()
        
        ### Add primitives (box, sphere, cylinder)
        pose = gen_pose(pos=[-0.9, 0.8, 0.5])
        psi.add_box("box", pose, size=(0.3, 0.5, 0.7))
        
        pose = gen_pose(pos=[0.0, 0.0, 2.5])
        psi.add_sphere("sphere", pose, radius=(0.3))
        
        # CollisionChecking with cylinder primitives does not work correctly, thus this is not supported! Use a mesh instead!
        
        
        ### ADDING A MESH
        #ToDo: use rospack to find package
        #ToDo: meshes cannot be scaled yet
        pose = gen_pose(pos=[0.6, 0.0, 0.9], euler=[-1.57, 0, 3.14])
        filename = '/opt/ros/groovy/stacks/simulator_gazebo/gazebo_worlds/Media/models/mug-test.stl'
        psi.add_mesh("mug", pose, filename)
        
        
        rospy.sleep(1.0)
        
        ### ADDING DETECT and URDF not supported anymore
        #ToDo: Add if needed
        
        
        ### ATTACHING AN OBJECT
        #ToDo: attach without creating objects again
        pose = gen_pose(pos=[0.4, 0.1, 1.0])
        psi.attach_box("tray_link", "box2", pose, (0.1,0.1,0.1))
        
        pose = gen_pose(pos=[0.5, -0.9, 0.9], euler=[-1.57, 0, 3.14])
        filename = '/opt/ros/groovy/stacks/simulator_gazebo/gazebo_worlds/Media/models/mug-test.stl'
        touch_links = ["sdh_grasp_link", "sdh_palm_link", "sdh_finger_11_link", "sdh_finger_12_link", "sdh_finger_13_link", "sdh_finger_21_link", "sdh_finger_22_link", "sdh_finger_23_link", "sdh_thumb_1_link", "sdh_thumb_2_link", "sdh_thumb_3_link"]
        psi.attach_mesh("arm_7_link", "mug", pose, filename, touch_links)
        
        
        rospy.sleep(1.0)
        
        ### DETACHING AN OBJECT
        #ToDo: detach without stating link?
        psi.remove_attached_object("arm_7_link", "mug")
        
        
        rospy.sleep(1.0)
        
        ### REMOVING AN OBJECT
        psi.remove_world_object("sphere")
        
        
        ### CLEAR ALL OBJECTS
        #ToDo: attached objects are not removed
        clear_objects()