clearAllObjects.py

Go to the documentation of this file.

#! /usr/bin/env python

PKG = 'pr2_arm_navigation_actions'

import roslib; roslib.load_manifest(PKG)
import rospy
import sys

from arm_navigation_msgs.msg import CollisionObject
from arm_navigation_msgs.msg import AttachedCollisionObject
from arm_navigation_msgs.msg import CollisionObjectOperation

if __name__ == '__main__':
    
    obj_pub = rospy.Publisher('collision_object',CollisionObject)
    att_pub = rospy.Publisher('attached_collision_object', AttachedCollisionObject)

    rospy.init_node('clear_all_objects')

    rospy.sleep(2.)

    co = CollisionObject()
    co.header.frame_id = "base_link"
    co.header.stamp = rospy.Time.now()

    co.id = "all"
    co.operation.operation = CollisionObjectOperation.REMOVE
    obj_pub.publish(co)

    ca = AttachedCollisionObject()
    ca.link_name = "all"
    ca.object = co
    att_pub.publish(ca)

    rospy.sleep(2.)