def form_attach_collision_object_msg(self, link_name, object_name, size, pose): obj = AttachedCollisionObject() # The CollisionObject will be attached with a fixed joint to this link obj.link_name = link_name # This contains the actual shapes and poses for the CollisionObject # to be attached to the link col_obj = CollisionObject() col_obj.id = object_name col_obj.header.frame_id = link_name col_obj.header.stamp = rospy.Time.now() sp = SolidPrimitive() sp.type = sp.BOX sp.dimensions = [0.0] * 3 sp.dimensions[0] = size.x sp.dimensions[1] = size.y sp.dimensions[2] = size.z col_obj.primitives = [sp] col_obj.primitive_poses = [pose] # Adds the object to the planning scene. If the object previously existed, it is replaced. col_obj.operation = col_obj.ADD obj.object = col_obj # The weight of the attached object, if known obj.weight = 0.0 return obj
def _add_payload_to_planning_scene_msg(self, payload): msg=payload.payload_msg co = CollisionObject() co.id = payload.payload_msg.name co.header.frame_id = msg.header.frame_id payload.attached_link=msg.header.frame_id urdf_root=self.urdf.get_root() urdf_chain=self.urdf.get_chain(urdf_root, payload.attached_link, joints=True, links=False) urdf_chain.reverse() touch_links=[] touch_root = None for j_name in urdf_chain: j=self.urdf.joint_map[j_name] if j.type != "fixed": break touch_root=j.parent def _touch_recurse(touch): ret=[touch] if touch in self.urdf.child_map: for c_j,c in self.urdf.child_map[touch]: if self.urdf.joint_map[c_j].type == "fixed": ret.extend(_touch_recurse(c)) return ret if touch_root is not None: touch_links.extend(_touch_recurse(touch_root)) for i in xrange(len(msg.collision_geometry.mesh_resources)): mesh_filename=urlparse.urlparse(resource_retriever.get_filename(msg.collision_geometry.mesh_resources[i])).path mesh_pose = rox_msg.transform2pose_msg(rox_msg.msg2transform(msg.pose) * rox_msg.msg2transform(msg.collision_geometry.mesh_poses[i])) mesh_scale=msg.collision_geometry.mesh_scales[i] mesh_scale = (mesh_scale.x, mesh_scale.y, mesh_scale.z) mesh_msg = load_mesh_file_to_mesh_msg(mesh_filename, mesh_scale) co.meshes.extend(mesh_msg) co.mesh_poses.extend([mesh_pose]*len(mesh_msg)) for i in xrange(len(msg.collision_geometry.primitives)): co.primitives.append(msg.collision_geometry.primitives[i]) primitive_pose = rox_msg.transform2pose_msg(rox_msg.msg2transform(msg.pose) * rox_msg.msg2transform(msg.collision_geometry.primitive_poses[i])) co.primitive_poses.append(primitive_pose) aco = AttachedCollisionObject() aco.link_name = payload.attached_link aco.object = co aco.touch_links = touch_links aco.weight = msg.inertia.m #self._pub_aco.publish(aco) return aco
def makeAttached(self, link_name, obj, touch_links, detach_posture, weight): o = AttachedCollisionObject() o.link_name = link_name o.object = obj if touch_links: o.touch_links = touch_links if detach_posture: o.detach_posture = detach_posture o.weight = weight return o
def makeAttached(self, link_name, obj, touch_links, detach_posture, weight): o = AttachedCollisionObject() o.link_name = link_name o.object = obj if touch_links: o.touch_links = touch_links if detach_posture: o.detach_posture = detach_posture o.weight = weight return o