diff --git a/libraries/scripts_library.sh b/libraries/scripts_library.sh
index 24cd2c3f67726a6b90ef82b3e842a32467b1916f..d5e1c0b44cd1c38f7eb3e0e94adc03f0695cc158 100644
--- a/libraries/scripts_library.sh
+++ b/libraries/scripts_library.sh
@@ -196,9 +196,14 @@ function find_ros_message
 
   if [[ ${ext} = "msg" ]] || [[ ${ext} = "srv" ]]
   then
-    my_file_tmp="${my_file%.*}"
-    ros_out=`eval "ros${ext} list | grep ${my_file_tmp}"`
-    #ros_out=`eval "ros${ext} show ${my_file}"`
+    if [[ ${my_file} == */* ]]
+    then
+      my_file_tmp="${my_file%.*}" #extract extension
+      ros_out=`eval "ros${ext} list | grep ${my_file_tmp}$"`
+    else
+      my_file_tmp="${my_file%.*}" #extract extension
+      ros_out=`eval "ros${ext} list | grep /${my_file_tmp}$"`
+    fi
   else
     #action file
     ros_out=`eval "rosmsg show ${my_file}Action"`
@@ -306,6 +311,30 @@ function find_comment_in_file
   fi
 }
 
+# text_found_in_file
+# Searches for $1 in $2. text_found is true if found.
+# - $1: text to be found in file
+# - $2: text file name
+function text_found_in_file
+{
+  if [[ -z $1 ]] || [[ -z $2 ]]
+  then
+    kill_exit "text_found_in_file missing input parameters"
+  fi
+
+  local text_to_find=$1
+  local file=$2
+
+  #look for key line
+  local found=$(grep -n "${text_to_find}" "${file}")
+  if [[ -n "${found}" ]]
+  then
+    text_found="true"
+  else
+    text_found="false"
+  fi
+}
+
 # add_line_to_file
 # Looks for comment line $2 in $3 file. If found, places line $1 after comment.
 # If comment is not found, exits script. If $1 already exists, skips adding it 
@@ -336,9 +365,6 @@ function add_line_to_file
 
   if [[ -z "${line_found}" ]]
   then
-    #add line to file
-#    echo "Modifying file ${file} with:"
-#    echo "     ${line_to_add}"
     sed -i -e "/${comment}/a\\${line_to_add}" "${file}"
   else
     #skip line
@@ -430,38 +456,75 @@ function check_package_file_integrity
 {
   local driver_alg=$1
   local comment=""
-
-  # check the package.xml file
+  
+  local type
+  
+  local build_depend="false"
+  local run_depend="false"
+  local depend="false"
+  local exec_depend="false"
+
+  
   if [[ "${driver_alg}" = "driver" ]]
   then
-    comment="<build_depend>iri_base_driver<\/build_depend>"
-    find_comment_in_file "${comment}" "package.xml"
-    if [[ "${comment_found}" = "false" ]]
-    then
-      kill_exit "<build_depend>iri_base_driver<\/build_depend> missing in package.xml file"
-    fi
+    type="driver"
   else
-    comment="<build_depend>iri_base_algorithm<\/build_depend>"
-    find_comment_in_file "${comment}" "package.xml"
-    if [[ "${comment_found}" = "false" ]]
-    then
-      kill_exit "<build_depend>iri_base_algorithm<\/build_depend> missing in package.xml file"
-    fi
+    type="algorithm"
   fi
-  if [[ "${driver_alg}" = "driver" ]]
+
+  comment="<build_depend>iri_base_${type}</build_depend>"
+  text_found_in_file "${comment}" "package.xml"
+  if [[ "${text_found}" = "false"  ]]
   then
-    comment="<run_depend>iri_base_driver<\/run_depend>"
-    find_comment_in_file "${comment}" "package.xml"
-    if [[ "${comment_found}" = "false" ]]
-    then
-      kill_exit "<run_depend>iri_base_driver<\/run_depend> missing in package.xml file"
-    fi
+    echo "${comment} not found in package.xml file"
   else
-    comment="<run_depend>iri_base_algorithm<\/run_depend>"
-    find_comment_in_file "${comment}" "package.xml"
-    if [[ "${comment_found}" = "false" ]]
-    then
-      kill_exit "<run_depend>iri_base_algorithm<\/run_depend> missing in package.xml file"
+    #echo "${comment} found in package.xml file"
+    build_depend=true
+  fi
+  
+  comment="<depend>iri_base_${type}</depend>"
+  text_found_in_file "${comment}" "package.xml"
+  if [[ "${text_found}" = "false"  ]]
+  then
+    echo "${comment} not found in package.xml file"
+  else
+    #echo "${comment} found in package.xml file"
+    depend=true
+  fi
+
+  comment="<exec_depend>iri_base_${type}</exec_depend>"
+  text_found_in_file "${comment}" "package.xml"
+  if [[ "${text_found}" = "false"  ]]
+  then
+    echo "${comment} not found in package.xml file"
+  else
+    #echo "${comment} found in package.xml file"
+    exec_depend="true"
+  fi
+  
+  comment="<run_depend>iri_base_${type}</run_depend>"
+  text_found_in_file "${comment}" "package.xml"
+  if [[ "${text_found}" = "false"  ]]
+  then
+    echo "${comment} not found in package.xml file"
+  else
+    #echo "${comment} found in package.xml file"
+    run_depend="true"
+  fi
+  
+  comment="<package format=\"2\">"
+  text_found_in_file "${comment}" "package.xml"
+  if [[ "${text_found}" = "true"  ]]
+  then
+    if [[ $build_depend != "true" || $exec_depend != "true" ]]; then
+      if [[ "$depend"!="true" ]]
+      then
+        kill_exit "Exit. Missing iri_base_$type <build_depend> or <exec_depend> on package.xml (format 2)"
+      fi
+    fi
+  else #format 1: <package>
+    if [ "$build_depend" != "true" ] || [ "$run_depend" != "true" ]; then
+      kill_exit "Exit. Missing iri_base_$type <build_depend> or <run_depend> on package.xml (format 1)"
     fi
   fi
 }
@@ -500,44 +563,62 @@ function add_build_run_dependencies
   local new_pkg=$3
   local line=""
   local comment=""
+  
+  local type=
+  local dep=
+  local dep2="build_depend"
+  
+  if [[ "${driver_alg}" = "driver" ]]
+  then
+    type="driver"
+  else
+    type="algorithm"
+  fi
 
   if [[ "${new_pkg}" != "${ros_pkg}" ]]
   then
-    line="<build_depend>${new_pkg}<\/build_depend>"
-    find_comment_in_file "${line}" "package.xml"
-    if [[ "${comment_found}" = "false" ]]
+  
+    comment="<package format=\"2\">"
+    text_found_in_file "${comment}" "package.xml"
+    if [[ "${text_found}" = "true"  ]]
     then
-      if [[ "${driver_alg}" = "driver" ]]
+      dep="depend"
+      line="<${dep}>${new_pkg}</${dep}>"
+      find_comment_in_file "${line}" "package.xml"
+      if [[ "${comment_found}" = "false" ]]
       then
-        line="<build_depend>${new_pkg}<\/build_depend>"
-        comment="<build_depend>iri_base_driver<\/build_depend>"
-        add_line_to_file "\ \ ${line}" "${comment}" "package.xml"
+        line="\ \ <${dep}>${new_pkg}<\/${dep}>"
+        comment="<${dep2}>iri_base_${type}<\/${dep2}>"
+        add_line_to_file "${line}" "${comment}" "package.xml"
       else
-        line="<build_depend>${new_pkg}<\/build_depend>"
-        comment="<build_depend>iri_base_algorithm<\/build_depend>"
-        add_line_to_file "\ \ ${line}" "${comment}" "package.xml"
+        echo "$dep dependency already included."
       fi
-    else
-      echo "Build dependencies already included."
-    fi
-
-    line="<run_depend>${new_pkg}<\/run_depend>"
-    find_comment_in_file "${line}" "package.xml"
-    if [[ "${comment_found}" = "false" ]]
-    then
-      if [[ "${driver_alg}" = "driver" ]]
+    else #format 1: <package>
+      dep="build_depend"
+      line="<${dep}>${new_pkg}</${dep}>"
+      find_comment_in_file "${line}" "package.xml"
+      if [[ "${comment_found}" = "false" ]]
       then
-        line="<run_depend>${new_pkg}<\/run_depend>"
-        comment="<run_depend>iri_base_driver<\/run_depend>"
-        add_line_to_file "\ \ ${line}" "${comment}" "package.xml"
+        line="\ \ <${dep}>${new_pkg}<\/${dep}>"
+        comment="<${dep2}>iri_base_${type}<\/${dep2}>"
+        add_line_to_file "${line}" "${comment}" "package.xml"
       else
-        line="<run_depend>${new_pkg}<\/run_depend>"
-        comment="<run_depend>iri_base_algorithm<\/run_depend>"
-        add_line_to_file "\ \ ${line}" "${comment}" "package.xml"
+        echo "$dep dependency already included."
+      fi
+      
+      dep="run_depend"
+      line="<${dep}>${new_pkg}</${dep}>"
+      find_comment_in_file "${line}" "package.xml"
+      if [[ "${comment_found}" = "false" ]]
+      then
+        line="\ \ <${dep}>${new_pkg}<\/${dep}>"
+        comment="<${dep2}>iri_base_${type}<\/${dep2}>"
+        add_line_to_file "${line}" "${comment}" "package.xml"
+      else
+        echo "$dep dependency already included."
       fi
-    else
-      echo "Run dependencies already included."
     fi
+
   fi
 }
 
@@ -566,7 +647,8 @@ function add_cmake_dependencies
         old_string="iri_base_algorithm"
       fi
       new_string="${old_string}\ ${new_pkg}"
-      sed -i "s/${old_string}/${new_string}/g" "CMakeLists.txt"
+      sed -i -e "/find_package/ s/${old_string}/${new_string}/g" "CMakeLists.txt"
+      sed -i -e "/CATKIN_DEPENDS/ s/${old_string}/${new_string}/g" "CMakeLists.txt"
     fi
 
     line="add_dependencies(\${PROJECT_NAME} ${new_pkg}_generate_messages_cpp)"