diff --git a/cffi_ros_utils/CMakeLists.txt b/cffi_ros_utils/CMakeLists.txt
new file mode 100644
index 0000000..f8c902e
--- /dev/null
+++ b/cffi_ros_utils/CMakeLists.txt
@@ -0,0 +1,5 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(cffi_ros_utils)
+find_package(catkin REQUIRED)
+
+catkin_package()
diff --git a/cffi_ros_utils/cffi-ros-utils.asd b/cffi_ros_utils/cffi-ros-utils.asd
new file mode 100644
index 0000000..428c415
--- /dev/null
+++ b/cffi_ros_utils/cffi-ros-utils.asd
@@ -0,0 +1,40 @@
+;;;
+;;; Copyright (c) 2010, Lorenz Moesenlechner <moesenle@in.tum.de>
+;;; All rights reserved.
+;;; 
+;;; Redistribution and use in source and binary forms, with or without
+;;; modification, are permitted provided that the following conditions are met:
+;;; 
+;;;     * Redistributions of source code must retain the above copyright
+;;;       notice, this list of conditions and the following disclaimer.
+;;;     * Redistributions in binary form must reproduce the above copyright
+;;;       notice, this list of conditions and the following disclaimer in the
+;;;       documentation and/or other materials provided with the distribution.
+;;;     * Neither the name of the Intelligent Autonomous Systems Group/
+;;;       Technische Universitaet Muenchen nor the names of its contributors 
+;;;       may be used to endorse or promote products derived from this software 
+;;;       without specific prior written permission.
+;;; 
+;;; THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+;;; AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+;;; IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+;;; ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+;;; LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+;;; CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+;;; SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+;;; INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+;;; CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+;;; ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+;;; POSSIBILITY OF SUCH DAMAGE.
+;;;
+
+(defsystem cffi-ros-utils
+  :author "Lorenz Moesenlechner <moesenle@in.tum.de>"
+  :license "BSD"
+
+  :depends-on (cffi cffi-grovel ros-load-manifest split-sequence)
+  :components
+  ((:module "src"
+            :components
+            ((:file "package")
+             (:file "ros-libraries" :depends-on ("package"))))))
diff --git a/cffi_ros_utils/package.xml b/cffi_ros_utils/package.xml
new file mode 100644
index 0000000..b7a1808
--- /dev/null
+++ b/cffi_ros_utils/package.xml
@@ -0,0 +1,27 @@
+<package>
+  <name>cffi_ros_utils</name>
+  <version>0.1.2</version>
+  <description>
+    ROS utility functions to use CFFI interface
+  </description>
+  
+  <author email="moesenle@cs.tum.edu">Lorenz Moesenlechner</author>
+
+  <maintainer email="moesenle@cs.tum.edu">Lorenz Moesenlechner</maintainer>
+  <maintainer email="georg.bartels@cs.uni-bremen.de">Georg Bartels</maintainer>
+ 
+  <license>BSD</license>
+  
+  <url type="bugtracker">https://github.com/cram-code/cram_3rdparty/issues</url>
+  <url type="repository">https://github.com/cram-code/cram_3rdparty</url>
+  
+  <buildtool_depend>catkin</buildtool_depend>
+  
+  <build_depend>cffi</build_depend>
+  <build_depend>split_sequence</build_depend>
+  <build_depend>sbcl</build_depend>
+  
+  <run_depend>cffi</run_depend>
+  <run_depend>split_sequence</run_depend>
+  <run_depend>sbcl</run_depend>
+</package>
diff --git a/cffi_ros_utils/src/package.lisp b/cffi_ros_utils/src/package.lisp
new file mode 100644
index 0000000..23b78ce
--- /dev/null
+++ b/cffi_ros_utils/src/package.lisp
@@ -0,0 +1,39 @@
+;;;
+;;; Copyright (c) 2010, Lorenz Moesenlechner <moesenle@in.tum.de>
+;;; All rights reserved.
+;;; 
+;;; Redistribution and use in source and binary forms, with or without
+;;; modification, are permitted provided that the following conditions are met:
+;;; 
+;;;     * Redistributions of source code must retain the above copyright
+;;;       notice, this list of conditions and the following disclaimer.
+;;;     * Redistributions in binary form must reproduce the above copyright
+;;;       notice, this list of conditions and the following disclaimer in the
+;;;       documentation and/or other materials provided with the distribution.
+;;;     * Neither the name of the Intelligent Autonomous Systems Group/
+;;;       Technische Universitaet Muenchen nor the names of its contributors 
+;;;       may be used to endorse or promote products derived from this software 
+;;;       without specific prior written permission.
+;;; 
+;;; THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+;;; AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+;;; IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+;;; ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+;;; LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+;;; CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+;;; SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+;;; INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+;;; CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+;;; ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+;;; POSSIBILITY OF SUCH DAMAGE.
+;;;
+
+(in-package :cl-user)
+
+(defpackage cffi-ros-utils
+  (:use #:common-lisp #:cffi)
+  (:export
+   :ros-library-paths
+   :ros-include-paths
+   :ros-grovel-file
+   :define-foreign-ros-library))
diff --git a/cffi_ros_utils/src/ros-libraries.lisp b/cffi_ros_utils/src/ros-libraries.lisp
new file mode 100644
index 0000000..42b9abd
--- /dev/null
+++ b/cffi_ros_utils/src/ros-libraries.lisp
@@ -0,0 +1,82 @@
+;;;
+;;; Copyright (c) 2010, Lorenz Moesenlechner <moesenle@in.tum.de>
+;;; All rights reserved.
+;;; 
+;;; Redistribution and use in source and binary forms, with or without
+;;; modification, are permitted provided that the following conditions are met:
+;;; 
+;;;     * Redistributions of source code must retain the above copyright
+;;;       notice, this list of conditions and the following disclaimer.
+;;;     * Redistributions in binary form must reproduce the above copyright
+;;;       notice, this list of conditions and the following disclaimer in the
+;;;       documentation and/or other materials provided with the distribution.
+;;;     * Neither the name of the Intelligent Autonomous Systems Group/
+;;;       Technische Universitaet Muenchen nor the names of its contributors 
+;;;       may be used to endorse or promote products derived from this software 
+;;;       without specific prior written permission.
+;;; 
+;;; THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+;;; AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+;;; IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+;;; ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+;;; LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+;;; CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+;;; SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+;;; INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+;;; CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+;;; ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+;;; POSSIBILITY OF SUCH DAMAGE.
+;;;
+
+(in-package :cffi-ros-utils)
+
+(defun ros-library-paths (pkg-name)
+  "Returns the list of library paths as exported by the ROS package
+  with name `pkg-name' in its cpp tag. This function parses the lflags
+  attribute and collects all library paths specified by -L."
+  (apply #'append (mapcar (lambda (seq)
+            (split-sequence:split-sequence #\: (subseq seq 2) :remove-empty-subseqs t))
+          (delete-if-not
+           (lambda (seq)
+             (equal (subseq seq 0 2) "-L"))
+           (split-sequence:split-sequence
+             #\Space
+             (car (ros-load:rospack "export" "--lang=cpp" "--attrib=lflags" pkg-name))
+             :remove-empty-subseqs t)))))
+
+(defun ros-include-paths (pkg-name)
+  "Returns the list of include paths as exported by the ROS package
+  `pkg-name'"
+  (split-sequence:split-sequence
+   #\Space
+   (car (ros-load:rospack "cflags-only-I" pkg-name))
+   :remove-empty-subseqs t))
+
+(defclass ros-grovel-file (cffi-grovel:grovel-file)
+  ((ros-package :initarg :ros-package)))
+
+(defmethod reinitialize-instance :after ((c ros-grovel-file)
+                                         &key (ros-package ros-load:*current-ros-package*))
+  (setf (cffi-grovel::cc-flags-of c)
+        (append (mapcar (lambda (path)
+                          (concatenate 'string "-I" path))
+                        (ros-include-paths ros-package))
+                (cffi-grovel::cc-flags-of c))))
+
+(defmacro define-foreign-ros-library (alias lib-file &optional (ros-package ros-load:*current-ros-package*))
+  (flet ((find-ros-library (file ros-package)
+           (let ((file-path (pathname file)))
+             (dolist (lib-path (ros-library-paths ros-package) nil)
+               (let ((complete-file-path (merge-pathnames
+                                          file-path
+                                          (make-pathname :directory lib-path))))
+                 (format t "~a~%" complete-file-path)
+                 (when (probe-file complete-file-path)
+                   (return-from find-ros-library complete-file-path)))))))
+    `(define-foreign-library ,alias
+       (:unix ,(namestring
+                (or (find-ros-library lib-file ros-package)
+                    (error
+                     'simple-error
+                     :format-control "Unable to find ros library `~a' in dependencies of package ~a"
+                     :format-arguments (list lib-file ros-package))))))))
diff --git a/cram_3rdparty/package.xml b/cram_3rdparty/package.xml
index 9cbfdb2..32f6b18 100644
--- a/cram_3rdparty/package.xml
+++ b/cram_3rdparty/package.xml
@@ -21,6 +21,7 @@
   <run_depend>alexandria</run_depend>
   <run_depend>babel</run_depend>
   <run_depend>cffi</run_depend>
+  <run_depend>cffi_ros_utils</run_depend>
   <run_depend>cl_store</run_depend>
   <run_depend>cl_utilities</run_depend>
   <run_depend>fiveam</run_depend>