diff --git a/stretch_funmap/CMakeLists.txt b/stretch_funmap/CMakeLists.txt
index 22ee6b2..c8c4bfa 100644
--- a/stretch_funmap/CMakeLists.txt
+++ b/stretch_funmap/CMakeLists.txt
@@ -11,6 +11,14 @@ find_package(catkin REQUIRED COMPONENTS
rospy
)
+set(cython_code_dir ${CMAKE_CURRENT_SOURCE_DIR}/src/stretch_funmap)
+
+message("Compile cython code within cmake build")
+execute_process(
+ WORKING_DIRECTORY ${cython_code_dir}
+ COMMAND bash ${cython_code_dir}/compile_cython_code.sh
+)
+
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
diff --git a/stretch_funmap/package.xml b/stretch_funmap/package.xml
index ac5a0ae..f3d53af 100644
--- a/stretch_funmap/package.xml
+++ b/stretch_funmap/package.xml
@@ -49,6 +49,7 @@
catkin
rospy
rospy
+ cython
rospy
ros_numpy
diff --git a/stretch_funmap/src/stretch_funmap/compile_cython_code.sh b/stretch_funmap/src/stretch_funmap/compile_cython_code.sh
index af33f98..faa0484 100755
--- a/stretch_funmap/src/stretch_funmap/compile_cython_code.sh
+++ b/stretch_funmap/src/stretch_funmap/compile_cython_code.sh
@@ -2,7 +2,10 @@
echo "Building cython_min_cost_path.pyx..."
cython ./cython_min_cost_path.pyx
-gcc -shared -pthread -fPIC -fwrapv -ffast-math -O3 -Wall -fno-strict-aliasing -I/usr/include/python2.7 -o cython_min_cost_path.so cython_min_cost_path.c
+
+PY_INCLUDE_PATH=$(python -c 'from sysconfig import get_paths as gp; print(gp()["include"])')
+echo "Using python lib: $PY_INCLUDE_PATH"
+gcc -shared -pthread -fPIC -fwrapv -ffast-math -O3 -Wall -fno-strict-aliasing -I$PY_INCLUDE_PATH -o cython_min_cost_path.so cython_min_cost_path.c
echo "Done."
echo ""