diff --git a/package.json b/package.json
index d53b6e64..0de7b70f 100644
--- a/package.json
+++ b/package.json
@@ -3,7 +3,7 @@
   "displayName": "ROS",
   "icon": "assets/icon.png",
   "description": "Development support for Robot Operating System (ROS)",
-  "version": "0.3.0",
+  "version": "0.3.10",
   "publisher": "ajshort",
   "license": "MIT",
   "repository": {
diff --git a/src/build.ts b/src/build.ts
index bab2b98a..15f1f94f 100644
--- a/src/build.ts
+++ b/src/build.ts
@@ -11,64 +11,90 @@ const PYTHON_AUTOCOMPLETE_PATHS = "python.autoComplete.extraPaths";
  * Creates config files which don't exist.
  */
 export async function createConfigFiles() {
-  const config = vscode.workspace.getConfiguration();
-
-  // Update the Python path if required.
-  if (config.get(PYTHON_AUTOCOMPLETE_PATHS, []).length === 0) {
-    updatePythonPath();
-  }
-
-  // Ensure the ".vscode" directory exists then update the C++ path.
-  const dir = path.join(vscode.workspace.rootPath, ".vscode");
-  
-  if (!await pfs.exists(dir)) {
-    await pfs.mkdir(dir);
-  }
-
-  pfs.exists(path.join(dir, "c_cpp_properties.json")).then(exists => {
-    if (!exists) {
-      updateCppProperties();
+    const config = vscode.workspace.getConfiguration();
+
+    // Update the Python path if required.
+    if (config.get(PYTHON_AUTOCOMPLETE_PATHS, []).length === 0) {
+        updatePythonPath();
     }
-  });
+
+    // Ensure the ".vscode" directory exists then update the C++ path.
+    const dir = path.join(vscode.workspace.rootPath, ".vscode");
+
+    if (!await pfs.exists(dir)) {
+        await pfs.mkdir(dir);
+    }
+
+    pfs.exists(path.join(dir, "c_cpp_properties.json")).then(exists => {
+        if (!exists) {
+            updateCppProperties();
+        }
+    });
 }
 
 /**
  * Updates the `c_cpp_properties.json` file with ROS include paths.
  */
 export async function updateCppProperties(): Promise<void> {
-  const includes = await utils.getIncludeDirs();
-  const filename = vscode.workspace.rootPath + "/.vscode/c_cpp_properties.json";
-
-  // Get all packages within the workspace, and check if they have an include
-  // directory. If so, add them to the list.
-  const packages = await utils.getPackages().then(
-    pkgs => _.values(pkgs).filter(pkg => pkg.startsWith(extension.baseDir))
-  );
-
-  await Promise.all(packages.map(pkg => {
-    const include = path.join(pkg, "include");
-
-    return pfs.exists(include).then(exists => {
-      if (exists) {
-        includes.push(include);
-      }
-    });
-  }));
-
-  await pfs.writeFile(filename, JSON.stringify({
-    configurations: [
-      {
-        browse: { databaseFilename: "", limitSymbolsToIncludedHeaders: true },
-        includePath: [...includes, "/usr/include"],
-        name: "Linux",
-      },
-    ],
-  }, undefined, 2));
+    const includes = await utils.getIncludeDirs();
+    const filename = vscode.workspace.rootPath + "/.vscode/c_cpp_properties.json";
+
+    // Get all packages within the workspace, and check if they have an include
+    // directory. If so, add them to the list.
+    const packages = await utils.getPackages().then(
+        pkgs => _.values(pkgs) //.filter(pkg => pkg.startsWith(extension.baseDir))
+    );
+
+    await Promise.all(packages.map(pkg => {
+        const include = path.join(pkg, "include");
+
+        return pfs.exists(include).then(exists => {
+            if (exists) {
+                includes.push(include);
+            }
+        });
+    }));
+
+    console.log( "cpp include: ", includes );
+
+    await pfs.writeFile(filename, JSON.stringify({
+        configurations: [
+            {
+                browse: { databaseFilename: "", limitSymbolsToIncludedHeaders: true },
+                includePath: [...includes, "/usr/include"],
+                name: "Linux",
+            },
+        ],
+    }, undefined, 2));
 }
 
 /**
  * Updates the python autocomplete path to support ROS.
  */
-export function updatePythonPath() {
-  vscode.workspace.getConfiguration().update(PYTHON_AUTOCOMPLETE_PATHS, extension.env.PYTHONPATH.split(":"));
+export async function updatePythonPath() {
+
+    const pathon_paths: string[] = [];
+
+    // Get all packages within the workspace, and check if they have an include
+    // directory. If so, add them to the list.
+    const packages = await utils.getPackages().then(
+        pkgs => _.values(pkgs) //.filter(pkg => pkg.startsWith(extension.baseDir))
+    );
+
+    await Promise.all(packages.map(pkg => {
+        const pkg_name = pkg.substring(pkg.lastIndexOf("/")+1);
+        const pkg_path = path.join(pkg, "src");
+
+        return pfs.exists( path.join(pkg_path, pkg_name, "__init__.py")).then(exists => {
+            if (exists) {
+                pathon_paths.push(pkg_path);
+            }
+        });
+    }));
+
+    await pfs.writeFile(vscode.workspace.rootPath + "/.env", "PYTHONPATH=" + pathon_paths.join(":"));
+
+    pathon_paths.push.apply(pathon_paths, process.env.PYTHONPATH.split(":"));
+
+    vscode.workspace.getConfiguration().update(PYTHON_AUTOCOMPLETE_PATHS, pathon_paths);
 }
diff --git a/src/catkin-task-provider.ts b/src/catkin-task-provider.ts
index eede9e45..bc58b9db 100644
--- a/src/catkin-task-provider.ts
+++ b/src/catkin-task-provider.ts
@@ -13,7 +13,7 @@ export default class CatkinTaskProvider implements vscode.TaskProvider {
       buildCommand = `catkin_make --directory "${extension.baseDir}"`;
       testCommand = `${buildCommand} run_tests`;
     } else if (extension.buildSystem === extension.BuildSystem.CatkinTools) {
-      buildCommand = `catkin build --workspace "${extension.baseDir}"`;
+      buildCommand = `catkin build -DCMAKE_BUILD_TYPE=Debug --workspace "${extension.baseDir}"`;
       testCommand = `${buildCommand} --catkin-make-args run_tests`;
     }