diff --git a/recipes-ros/octomap-ros/octomap-ros/0001-Add-build-dep-on-cmake_modules.patch b/recipes-ros/octomap-ros/octomap-ros/0001-Add-build-dep-on-cmake_modules.patch
new file mode 100644
index 0000000..8aba887
--- /dev/null
+++ b/recipes-ros/octomap-ros/octomap-ros/0001-Add-build-dep-on-cmake_modules.patch
@@ -0,0 +1,48 @@
+From f356c90e28477b6b5ec16e9941cd536eaf987186 Mon Sep 17 00:00:00 2001
+From: Dmitry Rozhkov <dmitry.rozhkov@linux.intel.com>
+Date: Wed, 25 Jan 2017 15:41:00 +0200
+Subject: [PATCH] Add build dep on cmake_modules
+
+The cmake module shipped with the octomap package uses absolute paths
+that break cross-compilation builds and require additional tweaking
+(e.g. see https://github.com/bmwcarit/meta-ros/blob/43bd17e8839f4078351895eca361804f19efb13c/recipes-ros/octomap-ros/octomap-ros_0.4.0.bb#L16)
+
+This patch makes octomap_ros use the cmake module provided by ROS's
+cmake_modules package to avoid such tweaks.
+
+Upstream-Status: Submitted [https://github.com/OctoMap/octomap_ros/pull/6]
+
+Signed-off-by: Dmitry Rozhkov <dmitry.rozhkov@linux.intel.com>
+---
+ CMakeLists.txt | 2 +-
+ package.xml    | 1 +
+ 2 files changed, 2 insertions(+), 1 deletion(-)
+
+diff --git a/CMakeLists.txt b/CMakeLists.txt
+index 7e86728..a91f6a7 100644
+--- a/CMakeLists.txt
++++ b/CMakeLists.txt
+@@ -1,7 +1,7 @@
+ cmake_minimum_required(VERSION 2.8)
+ project(octomap_ros)
+ 
+-find_package(catkin REQUIRED COMPONENTS sensor_msgs tf octomap_msgs)
++find_package(catkin REQUIRED COMPONENTS cmake_modules sensor_msgs tf octomap_msgs)
+ find_package(octomap REQUIRED)
+ 
+ catkin_package(
+diff --git a/package.xml b/package.xml
+index 69e0883..ab8f51d 100644
+--- a/package.xml
++++ b/package.xml
+@@ -15,6 +15,7 @@
+   <buildtool_depend>catkin</buildtool_depend>
+   
+   <build_depend>catkin</build_depend>
++  <build_depend>cmake_modules</build_depend>
+   <build_depend>octomap_msgs</build_depend>
+   <build_depend>octomap</build_depend>
+   <build_depend>sensor_msgs</build_depend>
+-- 
+2.7.4
+
diff --git a/recipes-ros/octomap-ros/octomap-ros_0.4.0.bb b/recipes-ros/octomap-ros/octomap-ros_0.4.0.bb
index 04847fe..d9c6d38 100644
--- a/recipes-ros/octomap-ros/octomap-ros_0.4.0.bb
+++ b/recipes-ros/octomap-ros/octomap-ros_0.4.0.bb
@@ -3,19 +3,14 @@ SECTION = "devel"
 LICENSE = "BSD"
 LIC_FILES_CHKSUM = "file://package.xml;beginline=11;endline=11;md5=d566ef916e9dedc494f5f793a6690ba5"
 
-DEPENDS = "octomap-msgs octomap pcl-ros sensor-msgs tf"
+DEPENDS = "cmake-modules octomap-msgs octomap pcl-ros sensor-msgs tf"
 
-SRC_URI = "https://github.com/OctoMap/${ROS_SPN}/archive/${PV}.tar.gz;downloadfilename=${ROS_SP}.tar.gz"
+SRC_URI = "https://github.com/OctoMap/${ROS_SPN}/archive/${PV}.tar.gz;downloadfilename=${ROS_SP}.tar.gz \
+           file://0001-Add-build-dep-on-cmake_modules.patch \
+           "
 SRC_URI[md5sum] = "4cd3fd7f2f94567ab6d37ee0ae756573"
 SRC_URI[sha256sum] = "6f4f90bbfe88c8314a406cfe70f0c4388d4d2d85a461d90911b2e6bf738ed153"
 
 S = "${WORKDIR}/${ROS_SP}"
 
 inherit catkin
-
-do_configure_append() {
-    sed -i -e 's: /usr/lib/liboctomap.so: ${STAGING_LIBDIR}/liboctomap.so:g' \
-        -e 's: /usr/lib/liboctomath.so: ${STAGING_LIBDIR}/liboctomath.so:g' \
-        ${B}/CMakeFiles/octomap_ros.dir/build.make
-    sed -i -e 's:-L\/opt\/ros\/indigo\/lib::g' ${B}/CMakeFiles/octomap_ros.dir/link.txt
-}