collision_map_interface.py
Go to the documentation of this file.
00001 #! /usr/bin/python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2009, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of the Willow Garage nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 #
00034 # author: Kaijen Hsiao
00035 
00036 ## @package collision_map_interface
00037 # Python version of collision_map_interface.cpp
00038 # Functions that take and reset the collision map, and add, remove, attach
00039 # or detach objects
00040 
00041 import roslib; roslib.load_manifest('tabletop_collision_map_processing')
00042 import random, time, math, scipy, pdb
00043 import rospy
00044 from arm_navigation_msgs.msg import Shape
00045 from tabletop_object_detector.msg import Table
00046 from arm_navigation_msgs.msg import MakeStaticCollisionMapAction, MakeStaticCollisionMapGoal
00047 from arm_navigation_msgs.msg import CollisionObject, AttachedCollisionObject, CollisionObjectOperation
00048 from actionlib_msgs.msg import GoalStatus
00049 from std_srvs.srv import Empty, EmptyRequest
00050 from arm_navigation_msgs.msg import Shape
00051 import actionlib
00052 import tf
00053 from geometry_msgs.msg import PoseStamped, Point, Quaternion, Pose
00054 from arm_navigation_msgs.srv import SetPlanningSceneDiff, SetPlanningSceneDiffRequest
00055 
00056 
00057 #interface to the collision map
00058 class CollisionMapInterface():
00059 
00060     def __init__(self):
00061 
00062         #set up service clients, publishers, and action clients        
00063         rospy.loginfo("advertising on collision object topics")
00064         self.object_in_map_pub = rospy.Publisher("collision_object", CollisionObject)
00065         self.attached_object_pub = rospy.Publisher("attached_collision_object", AttachedCollisionObject)
00066 
00067         rospy.loginfo("waiting for collision map reset service")
00068         rospy.wait_for_service("collider_node/reset")
00069         rospy.loginfo("found collision map reset service")
00070         self.reset_collision_map_srv = rospy.ServiceProxy("collider_node/reset", Empty)
00071 
00072         rospy.loginfo("waiting for set planning scene diff service")
00073         rospy.wait_for_service("environment_server/set_planning_scene_diff")
00074         self.set_planning_scene_srv = rospy.ServiceProxy("environment_server/set_planning_scene_diff", SetPlanningSceneDiff)
00075 
00076         self.object_in_map_current_id = 0
00077         rospy.loginfo("done initializing collision map interface")
00078 
00079 
00080     ##set the planning scene (so IK and move_arm can run)
00081     def set_planning_scene(self, ordered_collision_ops = None, link_padding = None):
00082         req = SetPlanningSceneDiffRequest()
00083         if ordered_collision_ops != None:
00084             req.operations = ordered_collision_ops
00085         if link_padding != None:
00086             req.planning_scene_diff.link_padding = link_padding
00087         try:
00088             resp = self.set_planning_scene_srv(req)
00089         except:
00090             rospy.logerr("error in calling set_planning_scene_diff!")
00091             return 0
00092         return 1
00093 
00094 
00095     ##clear everything in the octomap
00096     def clear_octomap(self):
00097         self.reset_collision_map_srv()
00098 
00099 
00100     ##reset all the current collision objects
00101     def reset_collision_map(self):
00102 
00103         #remove all objects
00104         reset_object = CollisionObject()
00105         reset_object.operation.operation = CollisionObjectOperation.REMOVE
00106         reset_object.header.frame_id = "base_link"
00107         reset_object.header.stamp = rospy.Time.now()
00108         reset_object.id = "all"
00109         self.object_in_map_pub.publish(reset_object)
00110 
00111         #and all attached objects
00112         reset_attached_objects = AttachedCollisionObject()
00113         reset_attached_objects.link_name = "all"
00114         reset_attached_objects.object.header.frame_id = "base_link"
00115         reset_attached_objects.object.header.stamp = rospy.Time.now()
00116         reset_attached_objects.object = reset_object
00117         self.attached_object_pub.publish(reset_attached_objects)
00118 
00119         #and clear the octomap
00120         self.clear_octomap()
00121 
00122         rospy.loginfo("collision objects reset")
00123         self.object_in_map_current_id = 0.
00124         return 1
00125 
00126     ##attaches an object to the gripper for the purposes of collision-aware 
00127     #motion planning
00128     def attach_object_to_gripper(self, object_name, whicharm = 'r'):
00129 
00130         obj = AttachedCollisionObject()
00131         
00132         obj.link_name = whicharm+"_gripper_r_finger_tip_link"
00133         obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
00134         obj.object.header.stamp = rospy.Time.now()
00135         obj.object.header.frame_id = "base_link"
00136         obj.object.id = object_name
00137         touch_links = ["_gripper_palm_link", "_gripper_r_finger_tip_link", "_gripper_l_finger_tip_link", "_gripper_l_finger_link", "_gripper_r_finger_link"]
00138         obj.touch_links = [whicharm+link for link in touch_links]
00139         self.attached_object_pub.publish(obj)
00140 
00141 
00142     ##detaches all objects from the gripper 
00143     #(removes from collision space entirely)
00144     def detach_all_objects_from_gripper(self, whicharm = 'r'):
00145 
00146         rospy.loginfo("detaching all objects from gripper %s"%whicharm)
00147         obj = AttachedCollisionObject()
00148         obj.object.header.stamp = rospy.Time.now()
00149         obj.object.header.frame_id = "base_link"
00150         obj.link_name = whicharm+"_gripper_r_finger_tip_link"
00151         obj.object.operation.operation = CollisionObjectOperation.REMOVE
00152         self.attached_object_pub.publish(obj)
00153         
00154 
00155     ##detaches all objects from the gripper 
00156     #(adds back as objects in the collision space where they are now)
00157     #object_collision_name is the original name for the collision object
00158     def detach_and_add_back_objects_attached_to_gripper(self, whicharm = 'r', object_collision_name = None):
00159 
00160         rospy.loginfo("detaching all objects from gripper %s and adding them back to the collision map"%whicharm)
00161 
00162         if object_collision_name == None:
00163             rospy.loginfo("need to specify the object name!  Detaching and not adding back object.")
00164             self.detach_all_objects_from_gripper(whicharm)
00165             return
00166 
00167         obj = AttachedCollisionObject()
00168         obj.object.header.stamp = rospy.Time.now()
00169         obj.object.header.frame_id = "base_link"
00170         obj.link_name = whicharm+"_gripper_r_finger_tip_link"
00171         obj.object.id = object_collision_name
00172         obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
00173         self.attached_object_pub.publish(obj)
00174 
00175 
00176     ##convert a Pose message to a 4x4 scipy matrix
00177     def pose_to_mat(self, pose):
00178         quat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
00179         pos = scipy.matrix([pose.position.x, pose.position.y, pose.position.z]).T
00180         mat = scipy.matrix(tf.transformations.quaternion_matrix(quat))
00181         mat[0:3, 3] = pos
00182         return mat
00183 
00184 
00185     #convert a 4x4 scipy matrix to a Pose message
00186     def mat_to_pose(self, mat):
00187         pose = Pose()
00188         pose.position.x = mat[0,3]
00189         pose.position.y = mat[1,3]
00190         pose.position.z = mat[2,3]
00191         quat = tf.transformations.quaternion_from_matrix(mat)
00192         pose.orientation.x = quat[0]
00193         pose.orientation.y = quat[1]
00194         pose.orientation.z = quat[2]
00195         pose.orientation.w = quat[3]
00196         return pose
00197 
00198 
00199     #adds the table to the map
00200     def process_collision_geometry_for_table(self, firsttable, additional_tables = []):
00201 
00202         table_object = CollisionObject()
00203         table_object.operation.operation = CollisionObjectOperation.ADD
00204         table_object.header.frame_id = firsttable.pose.header.frame_id
00205         table_object.header.stamp = rospy.Time.now()
00206 
00207         #create a box for each table
00208         for table in [firsttable,]+additional_tables:
00209             object = Shape()
00210             object.type = Shape.BOX;
00211             object.dimensions.append(math.fabs(table.x_max-table.x_min))
00212             object.dimensions.append(math.fabs(table.y_max-table.y_min))
00213             object.dimensions.append(0.01)
00214             table_object.shapes.append(object)
00215   
00216         #set the origin of the table object in the middle of the firsttable
00217         table_mat = self.pose_to_mat(firsttable.pose.pose)
00218         table_offset = scipy.matrix([(firsttable.x_min + firsttable.x_max)/2.0, (firsttable.y_min + firsttable.y_max)/2.0, 0.0]).T
00219         table_offset_mat = scipy.matrix(scipy.identity(4))
00220         table_offset_mat[0:3,3] = table_offset
00221         table_center = table_mat * table_offset_mat
00222         origin_pose = self.mat_to_pose(table_center)
00223         table_object.poses.append(origin_pose)
00224 
00225         table_object.id = "table"
00226         self.object_in_map_pub.publish(table_object)
00227 
00228 
00229     ##return the next unique collision object name
00230     def get_next_object_name(self):
00231         string = "graspable_object_"+str(self.object_in_map_current_id)
00232         self.object_in_map_current_id += 1
00233         return string
00234 
00235 
00236     ##adds a single box to the map
00237     def add_collision_box(self, box_pose, box_dims, frame_id, collision_name):
00238         
00239         rospy.loginfo("adding box to collision map")
00240         box = CollisionObject()
00241         box.operation.operation = CollisionObjectOperation.ADD
00242         box.header.frame_id = frame_id
00243         box.header.stamp = rospy.Time.now()
00244         shape = Shape()
00245         shape.type = Shape.BOX
00246         shape.dimensions = box_dims
00247         box.shapes.append(shape)
00248         box.poses.append(box_pose)
00249         box.id = collision_name
00250         self.object_in_map_pub.publish(box)
00251 
00252 
00253     ##adds a cluster to the map as a bunch of small boxes
00254     def process_collision_geometry_for_cluster(self, cluster):
00255 
00256         rospy.loginfo("adding cluster with %d points to collision map"%len(cluster.points))
00257 
00258         many_boxes = CollisionObject()
00259         
00260         many_boxes.operation.operation = CollisionObjectOperation.ADD
00261         many_boxes.header = cluster.header
00262         many_boxes.header.stamp = rospy.Time.now()
00263         num_to_use = int(len(cluster.points)/100.0)
00264         random_indices = range(len(cluster.points))
00265         scipy.random.shuffle(random_indices)
00266         random_indices = random_indices[0:num_to_use]
00267         for i in range(num_to_use):
00268             shape = Shape()
00269             shape.type = Shape.BOX
00270             shape.dimensions = [.005]*3
00271             pose = Pose()
00272             pose.position.x = cluster.points[random_indices[i]].x
00273             pose.position.y = cluster.points[random_indices[i]].y
00274             pose.position.z = cluster.points[random_indices[i]].z
00275             pose.orientation = Quaternion(*[0,0,0,1])
00276             many_boxes.shapes.append(shape)
00277             many_boxes.poses.append(pose)
00278         
00279         collision_name = self.get_next_object_name()
00280         many_boxes.id = collision_name
00281         self.object_in_map_pub.publish(many_boxes)
00282         return collision_name
00283 
00284 
00285     ##remove a single collision object from the map
00286     def remove_collision_object(self, collision_name):
00287 
00288         reset_object = CollisionObject()
00289         reset_object.operation.operation = CollisionObjectOperation.REMOVE
00290         reset_object.header.frame_id = "base_link"
00291         reset_object.header.stamp = rospy.Time.now()
00292         reset_object.id = collision_name
00293         self.object_in_map_pub.publish(reset_object) 


tabletop_collision_map_processing
Author(s): Matei Ciocarlie
autogenerated on Fri Jan 3 2014 11:49:29