diff --git a/add_action_server_client.sh b/add_action_server_client.sh
index 2cc0a3c8935e57b469743e89f0da4a696fcf9e9f..f0c05835e6c5f935491632ac547796af8533de7e 100755
--- a/add_action_server_client.sh
+++ b/add_action_server_client.sh
@@ -1,15 +1,17 @@
 #!/bin/bash
 
-# WET
+echo "/*********************************************/"
+echo "/*          Creating New ROS Action          */"
+echo "/*********************************************/"
 
 # check wether the scripts path environment variable has been defined
 scripts_path=`echo "${IRI_ROS_SCRIPTS_PATH}"`
 if [[ -z "${scripts_path}" ]]
 then
-  echo "The scripts path environment variable has not been defined. Please see the wiki documentation for instructions on how to create it."
+  echo "ERROR: The scripts path environment variable has not been defined. Please see the wiki documentation for instructions on how to create it."
   exit
-else
-  echo "The scripts path environment variable has been properly defined."
+# else
+#   echo "The scripts path environment variable has been properly defined."
 fi
 
 source "${IRI_ROS_SCRIPTS_PATH}/libraries/scripts_library.sh"
@@ -19,11 +21,6 @@ source "${IRI_ROS_SCRIPTS_PATH}/libraries/create_action_client.sh"
 check_libraries
 check_templates
 
-echo ""
-echo "/*********************************************/"
-echo "/*          Creating New ROS Action          */"
-echo "/*********************************************/"
-
 server_client=
 ros_pkg=
 action_name=
@@ -51,7 +48,7 @@ do
        action_file=$OPTARG
        ;;
     ?)
-       echo "invalid input argument ${OPTION}"
+       echo "ERROR: invalid input argument ${OPTION}"
        kill_exit "Usage: add_action_server_client.sh -o [server,client] -p ros_pkg -a action_name -m message.action"
        exit
        ;;
@@ -61,14 +58,14 @@ done
 #check if publisher name parameter is filled up
 if [ ! "${server_client}" ] || [ ! "${ros_pkg}" ] || [ ! "${action_name}" ] || [ ! "${action_file}" ]
 then
-  echo "Missing input parameters..."
+  echo "ERROR: Missing input parameters..."
   kill_exit "Usage: add_action_server_client.sh -o [server,client] -p ros_pkg -a action_name -m message.action"
 fi
 
 #check server client parameter
 if [[ ! "${server_client}" = "server" ]] && [[ ! "${server_client}" = "client" ]]
 then
-  kill_exit "First parameter must be either \"server\" or \"client\", aborting ..."
+  kill_exit "ERROR: First parameter must be either \"server\" or \"client\", aborting ..."
 fi
 
 #check if package exists
@@ -77,7 +74,7 @@ if [[ ${pkg_exists} == true ]]
 then
   roscd ${ros_pkg}
 else
-  kill_exit "ROS package ${ros_pkg} does NOT exist yet, please first run iri_ros_create_package.sh"
+  kill_exit "ERROR: ROS package ${ros_pkg} does NOT exist yet, please first run iri_ros_create_package.sh"
 fi
 
 #validate file extension .action
@@ -89,7 +86,7 @@ action_file=${action_file%.*}
 #check extension
 if [[ ! "${ext}" = "${act}" ]]
 then
-  kill_exit "Wrong file extension, please provide a .action file, aborting ..."
+  kill_exit "ERROR: Wrong file extension, please provide a .action file, aborting ..."
 fi
 
 #look for ACTION file
@@ -99,8 +96,7 @@ then
   action_file=${my_file}
   echo "ACTION file ${action_file} found!"
 else
-  echo "ACTION file ${action_file} does NOT exist, please check if file is in valid directories"
-  kill_exit "Aborting ..."
+  kill_exit "ERROR. ACTION file ${action_file} does NOT exist, please check if file is in valid directories, aborting ..."
 fi
 
 #retrieve header and source files and pkg kind (algorithm/driver)
@@ -112,7 +108,7 @@ echo "driver_alg=${driver_alg}"
 
 if [[ -z ${node_h} ]] || [[ -z ${node_c} ]] || [[ -z ${driver_alg} ]]
 then
-  kill_exit "Problems with headers and/or source files"
+  kill_exit "ERROR: Problems found with headers and/or source files"
 fi
 
 #go to package folder
diff --git a/add_service_server_client.sh b/add_service_server_client.sh
index bae8fc583387f4d0d31ad8acd3c150023cd8fee5..bfb820f416c5f205f660498ad0c3f0d2a10533e1 100755
--- a/add_service_server_client.sh
+++ b/add_service_server_client.sh
@@ -1,15 +1,17 @@
 #!/bin/bash
 
-# WET
+echo "/*********************************************/"
+echo "/*      Creating New ROS Server/Client       */"
+echo "/*********************************************/"
 
 # check wether the scripts path environment variable has been defined
 scripts_path=`echo "${IRI_ROS_SCRIPTS_PATH}"`
 if [[ -z "${scripts_path}" ]]
 then
-  echo "The scripts path environment variable has not been defined. Please see the wiki documentation for instructions on how to create it."
+  echo "ERROR: The scripts path environment variable has not been defined. Please see the wiki documentation for instructions on how to create it."
   exit
-else
-  echo "The scripts path environment variable has been properly defined."
+# else
+#   echo "The scripts path environment variable has been properly defined."
 fi
 
 source "${IRI_ROS_SCRIPTS_PATH}/libraries/scripts_library.sh"
@@ -19,11 +21,6 @@ source "${IRI_ROS_SCRIPTS_PATH}/libraries/create_client.sh"
 check_libraries
 check_templates
 
-echo ""
-echo "/*********************************************/"
-echo "/*      Creating New ROS Server/Client       */"
-echo "/*********************************************/"
-
 server_client=
 ros_pkg=
 service_name=
@@ -51,7 +48,7 @@ do
        srv_file=$OPTARG
        ;;
     ?)
-       echo "invalid input argument ${OPTION}"
+       echo "ERROR: invalid input argument ${OPTION}"
        kill_exit "Usage: add_service_server_client.sh -o [server,client] -p ros_pkg -s service_name -m service.srv"
        exit
        ;;
@@ -61,14 +58,14 @@ done
 #check if publisher name parameter is filled up
 if [ ! "${server_client}" ] || [ ! "${ros_pkg}" ] || [ ! "${service_name}" ] || [ ! "${srv_file}" ]
 then
-  echo "Missing input parameters..."
+  echo "ERROR: Missing input parameters..."
   kill_exit "Usage: add_service_server_client.sh -o [server,client] -p ros_pkg -s service_name -m service.srv"
 fi
 
 #check server client parameter
 if [[ ! "${server_client}" = "server" ]] && [[ ! "${server_client}" = "client" ]]
 then
-  kill_exit "First parameter must be either \"server\" or \"client\", aborting ..."
+  kill_exit "ERROR: First parameter must be either \"server\" or \"client\", aborting ..."
 fi
 
 #check if package exists
@@ -77,7 +74,7 @@ if [[ ${pkg_exists} == true ]]
 then
   roscd ${ros_pkg}
 else
-  kill_exit "ROS package ${ros_pkg} does NOT exist yet, please first run iri_ros_create_package.sh"
+  kill_exit "ERROR: ROS package ${ros_pkg} does NOT exist yet, please first run iri_ros_create_package.sh"
 fi
 
 #validate file extension .srv
@@ -88,7 +85,7 @@ ext=$(echo ${ext} | tr "[:upper:]" "[:lower:]")
 #check extension
 if [[ ! "${ext}" = "${srv}" ]]
 then
-  kill_exit "Wrong file extension, please provide a .srv file, aborting ..."
+  kill_exit "ERROR: Wrong file extension, please provide a .srv file, aborting ..."
 fi
 
 #look for SRV file
@@ -98,8 +95,7 @@ then
   srv_file=${my_file}
   echo "SRV file ${srv_file} found!"
 else
-  echo "SRV file ${srv_file} does NOT exist, please check if file is in valid directories"
-  kill_exit "Aborting ..."
+  kill_exit "ERROR: SRV file ${srv_file} does NOT exist, please check if file is in valid directories, aborting ..."
 fi
 
 #retrieve header and source files and pkg kind (algorithm/driver)
@@ -111,7 +107,7 @@ echo "driver_alg=${driver_alg}"
 
 if [[ -z ${node_h} ]] || [[ -z ${node_c} ]] || [[ -z ${driver_alg} ]]
 then
-  kill_exit "Problems with headers and/or source files"
+  kill_exit "ERROR: Problems found with headers and/or source files"
 fi
 
 #go to package folder
diff --git a/add_tf_listener_broadcaster.sh b/add_tf_listener_broadcaster.sh
index 8f8a9278b35abb535eb52bcac55cc5d140416145..263bff1671ab991e1b9dbad40e3a7641af307cf0 100755
--- a/add_tf_listener_broadcaster.sh
+++ b/add_tf_listener_broadcaster.sh
@@ -1,6 +1,8 @@
 #!/bin/bash
 
-# # WET
+echo "/*********************************************/"
+echo "/*   Creating New ROS TF Listener broadcaster */"
+echo "/*********************************************/"
 
 script_name="add_tf_listener_broadcaster"
 
@@ -8,10 +10,10 @@ script_name="add_tf_listener_broadcaster"
 scripts_path=`echo "${IRI_ROS_SCRIPTS_PATH}"`
 if [[ -z "${scripts_path}" ]]
 then
-  echo "The scripts path environment varibale has not been defined. Please see the wiki documentation for instructions on how to create it."
+  echo "ERROR: The scripts path environment varibale has not been defined. Please see the wiki documentation for instructions on how to create it."
   exit
-else
-  echo "The scripts path environment variable has been properly defined."
+# else
+#   echo "The scripts path environment variable has been properly defined."
 fi
 
 source "${IRI_ROS_SCRIPTS_PATH}/libraries/scripts_library.sh"
@@ -21,11 +23,6 @@ source "${IRI_ROS_SCRIPTS_PATH}/libraries/create_tf_listener.sh"
 check_libraries
 check_templates
 
-echo ""
-echo "/*********************************************/"
-echo "/*   Creating New ROS TF Listener broadcaster */"
-echo "/*********************************************/"
-
 pub_subs=
 ros_pkg=
 
@@ -41,7 +38,7 @@ do
        ;;
 
     ?)
-       echo "invalid input argument ${OPTION}"
+       echo "ERROR: invalid input argument ${OPTION}"
        kill_exit "Usage: ${script_name}.sh -o [broadcaster,listener] -p ros_pkg"
        exit
        ;;
@@ -51,14 +48,14 @@ done
 #check if publisher name parameter is filled up
 if [ ! "${pub_subs}" ] || [ ! "${ros_pkg}" ]
 then
-  echo "Missing input parameters..."
+  echo "ERROR: Missing input parameters..."
   kill_exit "Usage: ${script_name}.sh -o [broadcaster,listener] -p ros_pkg"
 fi
 
 #check publisher subscriber parameter
 if [[ ! "${pub_subs}" = "broadcaster" ]] && [[ ! "${pub_subs}" = "listener" ]]
 then
-  kill_exit "First parameter must be either \"broadcaster\" or \"listener\", aborting ..."
+  kill_exit "ERROR: First parameter must be either \"broadcaster\" or \"listener\", aborting ..."
 fi
 
 #check if package exists
@@ -67,7 +64,7 @@ if [[ -z "${result}" ]]
 then
   roscd ${ros_pkg}
 else
-  kill_exit "ROS package ${ros_pkg} does NOT exist or can't be found. Create it, or load the correct workspace."
+  kill_exit "ERROR: ROS package ${ros_pkg} does NOT exist or can't be found. Create it, or load the correct workspace."
 fi
 
 check_package "${ros_pkg}"
@@ -87,7 +84,7 @@ echo "driver_alg=${driver_alg}"
 
 if [[ -z ${node_h} ]] || [[ -z ${node_c} ]] || [[ -z ${driver_alg} ]]
 then
-  kill_exit "Problems with headers and/or source files"
+  kill_exit "ERROR: Problems found with headers and/or source files"
 fi
 
 #go to package folder
@@ -99,5 +96,4 @@ then
   create_tf_broadcaster  ${ros_pkg} ${node_h} ${node_c} ${driver_alg}
 else
   create_tf_listener     ${ros_pkg} ${node_h} ${node_c} ${driver_alg}
-fi
-
+fi
\ No newline at end of file
diff --git a/add_topic_publisher_subscriber.sh b/add_topic_publisher_subscriber.sh
index 2a27b5a3ced7b3d881d74e825f3dbcd503ef1ff7..17a44615bdc82bc78a0f6646115ca7d8259a67c3 100755
--- a/add_topic_publisher_subscriber.sh
+++ b/add_topic_publisher_subscriber.sh
@@ -1,15 +1,17 @@
 #!/bin/bash
 
-# WET
+echo "/*********************************************/"
+echo "/*   Creating New ROS Publisher/Subscriber   */"
+echo "/*********************************************/"
 
 # check wether the scripts path environment variable has been defined
 scripts_path=`echo "${IRI_ROS_SCRIPTS_PATH}"`
 if [[ -z "${scripts_path}" ]]
 then
-  echo "The scripts path environment varibale has not been defined. Please see the wiki documentation for instructions on how to create it."
+  echo "ERROR: The scripts path environment varibale has not been defined. Please see the wiki documentation for instructions on how to create it."
   exit
-else
-  echo "The scripts path environment variable has been properly defined."
+# else
+#   echo "The scripts path environment variable has been properly defined."
 fi
 
 source "${IRI_ROS_SCRIPTS_PATH}/libraries/scripts_library.sh"
@@ -19,11 +21,6 @@ source "${IRI_ROS_SCRIPTS_PATH}/libraries/create_subscriber.sh"
 check_libraries
 check_templates
 
-echo ""
-echo "/*********************************************/"
-echo "/*   Creating New ROS Publisher/Subscriber   */"
-echo "/*********************************************/"
-
 pub_subs=
 ros_pkg=
 topic_name=
@@ -55,7 +52,7 @@ do
        buffer=$OPTARG
        ;;
     ?)
-       echo "invalid input argument ${OPTION}"
+       echo "ERROR: invalid input argument ${OPTION}"
        kill_exit "Usage: add_topic_publisher_subscriber.sh -o [publisher,subscriber] -p ros_pkg -t topic_name -m message.msg -b 1"
        exit
        ;;
@@ -65,14 +62,14 @@ done
 #check if publisher name parameter is filled up
 if [ ! "${pub_subs}" ] || [ ! "${ros_pkg}" ] || [ ! "${topic_name}" ] || [ ! "${msg_file}" ] || [ ! "${buffer}" ]
 then
-  echo "Missing input parameters..."
+  echo "ERROR: Missing input parameters..."
   kill_exit "Usage: add_topic_publisher_subscriber.sh -o [publisher,subscriber] -p ros_pkg -t topic_name -m message.msg -b 1"
 fi
 
 #check publisher subscriber parameter
 if [[ ! "${pub_subs}" = "publisher" ]] && [[ ! "${pub_subs}" = "subscriber" ]]
 then
-  kill_exit "First parameter must be either \"publisher\" or \"subscriber\", aborting ..."
+  kill_exit "ERROR: First parameter must be either \"publisher\" or \"subscriber\", aborting ..."
 fi
 
 #check if package exists
@@ -81,7 +78,7 @@ if [[ -z "${result}" ]]
 then
   roscd ${ros_pkg}
 else
-  kill_exit "ROS package ${ros_pkg} does NOT exist yet, please first run iri_ros_create_package.sh"
+  kill_exit "ERROR: ROS package ${ros_pkg} does NOT exist yet, please first run iri_ros_create_package.sh"
 fi
 
 check_package "${ros_pkg}"
@@ -89,7 +86,7 @@ if [[ ${pkg_exists} == true ]]
 then
   roscd ${ros_pkg}
 else
-  kill_exit "ROS package ${ros_pkg} does NOT exist yet, please first run iri_ros_create_package.sh"
+  kill_exit "ERROR: ROS package ${ros_pkg} does NOT exist yet, please first run iri_ros_create_package.sh"
 fi
 
 #validate file extension .msg
@@ -100,7 +97,7 @@ ext=$(echo ${ext} | tr "[:upper:]" "[:lower:]")
 #check extension
 if [[ ! "${ext}" = "${msg}" ]]
 then
-  kill_exit "Wrong file extension, please provide a .msg file, aborting ..."
+  kill_exit "ERROR: Wrong file extension, please provide a .msg file, aborting ..."
 fi
 
 #look for MSG file
@@ -110,8 +107,7 @@ then
   msg_file=${my_file}
   echo "MSG file ${msg_file} found!"
 else
-  echo "MSG file ${msg_file} does NOT exist, please check if file is in valid directories"
-  kill_exit "Aborting ..."
+  kill_exit "ERROR: MSG file ${msg_file} does NOT exist, please check if file is in valid directories, aborting ..."
 fi
 
 # Sanitize input and assign to new variable
@@ -122,7 +118,7 @@ if [ "$clean_buffer" ] && [ "$clean_buffer -gt 0" ]
 then
   echo "Setting buffer length to $clean_buffer"
 else
-  kill_exit "No buffer provided, aborting ..."  
+  kill_exit "ERROR: No buffer provided, aborting ..."  
 fi
 
 #retrieve header and source files and pkg kind (algorithm/driver)
@@ -134,7 +130,7 @@ echo "driver_alg=${driver_alg}"
 
 if [[ -z ${node_h} ]] || [[ -z ${node_c} ]] || [[ -z ${driver_alg} ]]
 then
-  kill_exit "Problems with headers and/or source files"
+  kill_exit "ERROR: Problems found with headers and/or source files"
 fi
 
 #go to package folder
diff --git a/algorithm_templates/template_alg_node.cpp b/algorithm_templates/template_alg_node.cpp
index 2748a0309b89b2dc1ffc7ceb2f12282d0e939549..9cfc745ed63a0bf526e96e1e23f78ed2f0fc53ee 100644
--- a/algorithm_templates/template_alg_node.cpp
+++ b/algorithm_templates/template_alg_node.cpp
@@ -31,7 +31,9 @@ TemplateNode::~TemplateNode(void)
 
 void TemplateNode::mainNodeThread(void)
 {
-  ROS_INFO("TemplateNode::mainNodeThread");
+  //lock access to algorithm if necessary
+  this->alg_.lock();
+  ROS_DEBUG("TemplateNode::mainNodeThread");
   // [fill msg structures]
   
   // [fill srv structure and make request to the server]
@@ -39,6 +41,8 @@ void TemplateNode::mainNodeThread(void)
   // [fill action structure and make request to the action server]
 
   // [publish messages]
+  
+  this->alg_.unlock();
 }
 
 /*  [subscriber callbacks] */
diff --git a/create_algorithm_package.sh b/create_algorithm_package.sh
index 68d13cb5ffa813b2f177a4f5d95e1a6fc2ceed4e..745e310060e7deded2ba333870959a6166a5b9c0 100755
--- a/create_algorithm_package.sh
+++ b/create_algorithm_package.sh
@@ -1,15 +1,17 @@
 #!/bin/bash
 
-# WET
+echo "/**********************************************/"
+echo "/* Creating New IRI_ROS Simple Algorithm Node */"
+echo "/**********************************************/"
 
 # check wether the scripts path environment variable has been defined
 scripts_path=`echo "${IRI_ROS_SCRIPTS_PATH}"`
 if [[ -z "${scripts_path}" ]]
 then
-  echo "The scripts path environment variable has not been defined. Please see the wiki documentation for instructions on how to create it."
+  echo "ERROR: The scripts path environment variable has not been defined. Please see the wiki documentation for instructions on how to create it."
   exit
-else
-  echo "The scripts path environment variable has been properly defined."
+# else
+#   echo "The scripts path environment variable has been properly defined."
 fi
 
 source "${IRI_ROS_SCRIPTS_PATH}/libraries/scripts_library.sh"
@@ -17,11 +19,6 @@ source "${IRI_ROS_SCRIPTS_PATH}/libraries/scripts_library.sh"
 check_libraries
 check_templates
 
-echo ""
-echo "/**********************************************/"
-echo "/* Creating New IRI_ROS Simple Algorithm Node */"
-echo "/**********************************************/"
-
 usage="Usage: create_algorithm_package.sh -n node_name [-i] [-p <prefix>]\n
 Parameters:\n
 \t-i: avoid adding the default iri_ prefix to the provided node name.\n
@@ -45,7 +42,7 @@ do
        input_prefix=$OPTARG
        ;;
     ?)
-       echo "invalid argument $OPTION"
+       echo "ERROR: invalid argument $OPTION"
        kill_exit "${usage}"
        ;;
   esac
@@ -54,7 +51,7 @@ done
 #check if project parameter is filled up
 if [ ! "${input_name}" ]
 then
-  echo "No project name provided, aborting ..."
+  echo "ERROR: No project name provided, aborting ..."
   kill_exit "${usage}"
 fi
 
@@ -119,8 +116,7 @@ sed -e "s/template_alg/${alg_filename}/g" \
     -e "s/TemplateConfig/${basename}Config/g" <${temps_folder}/template_alg.h >"${project_name}/include/${alg_filename}.h"
 sed -e "s/template_alg/${alg_filename}/g" \
     -e "s/TemplateAlg/${alg_basename}/g" <${temps_folder}/template_alg.cpp >"${project_name}/src/${alg_filename}.cpp"
-echo "Creating ${alg_filename} files..."
-echo ""
+echo "Creating ${alg_filename}.h/cpp files..."
 ################################################################################
 
 
@@ -140,8 +136,7 @@ sed -e "s/template_alg/${alg_filename}/g" \
 sed -e "s/template_node/${node_filename}/g" \
     -e "s/TemplateAlg/${alg_basename}/g" \
     -e "s/TemplateNode/${node_basename}/g" <${temps_folder}/template_alg_node.cpp >"${project_name}/src/${node_filename}.cpp"
-echo "Creating ${node_filename} files..."
-echo ""
+echo "Creating ${node_filename}.h/cpp files..."
 ################################################################################
 
 
@@ -150,8 +145,7 @@ echo ""
 sed -e "s/template_alg/${alg_filename}/g" \
     -e "s/Template/${basename}/g" \
     -e "s/template_node/${project_name}/g" <${temps_folder}/CMakeLists.txt >"${project_name}/CMakeLists.txt"
-echo "Creating ${project_name} CMakeLists.txt file..."
-echo ""
+echo "Creating ${project_name}/CMakeLists.txt file..."
 ################################################################################
 
 
@@ -172,8 +166,7 @@ sed -e "s/template/${project_name}/g" \
     -e "s/template_node/${node_filename}/g" \
     -e "s/Template/${basename}/g" <${temps_folder}/template_alg.cfg >"${project_name}/cfg/${basename}.cfg"
 eval "chmod 775 ${project_name}/cfg/${basename}.cfg"
-echo "Creating ${cfg_filename}.cfg file..."
-echo ""
+echo "Creating ${cfg_dir}/${cfg_filename}.cfg file..."
 ################################################################################
 
 
@@ -195,16 +188,12 @@ cp ${IRI_ROS_SCRIPTS_PATH}/common_templates/LICENSE ${project_name}/LICENSE
 
 sed -e "s/template/${project_name}/g" < ${IRI_ROS_SCRIPTS_PATH}/common_templates/README.md > "${project_name}/README.md"
 ################################################################################
-
-
+pushd "${project_name}" > /dev/null
+change_license_to_LGPL
+popd > /dev/null
 ################################################################################
-echo ""
 echo "Project ${project_name} has been successfully created!!"
-
-pushd "${project_name}"
-change_license_to_LGPL
-popd
-
-# WET
+echo ""
+echo "Compiling workspace with: catkin_make --only-pkg-with-deps $project_name"
 goto_catkin_workspace
 catkin_make --only-pkg-with-deps ${project_name}
\ No newline at end of file
diff --git a/create_driver_package.sh b/create_driver_package.sh
index 806e64673ab24223c6e44df50968f2ffc7acd8d7..4429a9b8514d2a91a237fe9891f7696dad489541 100755
--- a/create_driver_package.sh
+++ b/create_driver_package.sh
@@ -1,15 +1,17 @@
 #!/bin/bash
 
-# WET
+echo "/*********************************************/"
+echo "/*     Creating New IRI_ROS Driver Node      */"
+echo "/*********************************************/"
 
 # check wether the scripts path environment variable has been defined
 scripts_path=`echo "${IRI_ROS_SCRIPTS_PATH}"`
 if [[ -z "${scripts_path}" ]]
 then
-  echo "The scripts path environment variable has not been defined. Please see the wiki documentation for instructions on how to create it."
+  echo "ERROR: The scripts path environment variable has not been defined. Please see the wiki documentation for instructions on how to create it."
   exit
-else
-  echo "The scripts path environment variable has been properly defined."
+# else
+#   echo "The scripts path environment variable has been properly defined."
 fi
 
 source "${IRI_ROS_SCRIPTS_PATH}/libraries/scripts_library.sh"
@@ -17,11 +19,6 @@ source "${IRI_ROS_SCRIPTS_PATH}/libraries/scripts_library.sh"
 check_libraries
 check_templates
 
-echo ""
-echo "/*********************************************/"
-echo "/*     Creating New IRI_ROS Driver Node      */"
-echo "/*********************************************/"
-
 usage="Usage: create_driver_package.sh -n node_name [-i [other_prefix]]
 Parameters:\n
 \t-i: avoid adding the default iri_ prefix to the provided node name.\n
@@ -42,7 +39,7 @@ do
        input_prefix=$OPTARG
        ;;
     ?)
-       echo "invalid argument $OPTION"
+       echo "ERROR: invalid argument $OPTION"
        kill_exit "${usage}"
        ;;
   esac
@@ -51,7 +48,7 @@ done
 #check if project parameter is filled up
 if [ ! "${input_name}" ]
 then
-  echo "No project name provided, aborting ..."
+  echo "ERROR: No project name provided, aborting ..."
   kill_exit "${usage}"
 fi
 
@@ -73,7 +70,7 @@ fi
 
 if [ -e "../${project_name}" ]
 then
-  kill_exit "${project_name} package directory already exists, aborting ..."
+  kill_exit "ERROR: ${project_name} package directory already exists, aborting ..."
 else
   echo "Generating folder structure for project ${project_name} ..."
 fi
@@ -120,7 +117,6 @@ sed -e "s/template_driver/${driver_filename}/g" \
 sed -e "s/template_driver/${driver_filename}/g" \
     -e "s/TemplateDriver/${driver_basename}/g" <${IRI_ROS_SCRIPTS_PATH}/driver_templates/template_driver.cpp >"${project_name}/src/${driver_filename}.cpp"
 echo "Creating ${driver_filename} files..."
-echo ""
 ################################################################################
 
 
@@ -142,7 +138,6 @@ sed -e "s/template_driver/${driver_filename}/g" \
     -e "s/template_node/${node_filename}/g" \
     -e "s/TemplateNode/${node_basename}/g" <${IRI_ROS_SCRIPTS_PATH}/driver_templates/template_driver_node.cpp >"${project_name}/src/${node_filename}.cpp"
 echo "Creating ${node_filename} files..."
-echo ""
 ################################################################################
 
 
@@ -152,7 +147,6 @@ sed -e "s/template_driver/${driver_filename}/g" \
     -e "s/template_node/${project_name}/g" \
     -e "s/Template/${basename}/g" <${IRI_ROS_SCRIPTS_PATH}/driver_templates/CMakeLists.txt >"${project_name}/CMakeLists.txt" 
 echo "Creating ${project_name} CMakeLists.txt file..."
-echo ""
 ################################################################################
 
 
@@ -175,7 +169,8 @@ sed -e "s/template_driver/${driver_filename}/g" \
     -e "s/Template/${basename}/g" <${IRI_ROS_SCRIPTS_PATH}/driver_templates/template_driver.cfg >"${project_name}/cfg/${basename}.cfg"
 eval "chmod 775 ${project_name}/cfg/${basename}.cfg"
 echo "Creating ${basename}.cfg file..."
-echo ""
+################################################################################
+
 
 ################################################################################
 #create launch/config directory
@@ -195,13 +190,12 @@ cp ${IRI_ROS_SCRIPTS_PATH}/common_templates/LICENSE ${project_name}/LICENSE
 
 sed -e "s/template/${project_name}/g" < ${IRI_ROS_SCRIPTS_PATH}/common_templates/README.md > "${project_name}/README.md"
 ################################################################################
-
-echo ""
-echo "Project ${project_name} has been successfully created!!"
-
-pushd "${project_name}"
+pushd "${project_name}" > /dev/null
 change_license_to_LGPL
-popd
-
+popd > /dev/null
+################################################################################
+echo "Project ${project_name} has been successfully created!!"
+echo ""
+echo "Compiling workspace with: catkin_make --only-pkg-with-deps $project_name"
 goto_catkin_workspace
-catkin_make --only-pkg-with-deps ${project_name}
+catkin_make --only-pkg-with-deps ${project_name}
\ No newline at end of file
diff --git a/driver_templates/template_driver_node.cpp b/driver_templates/template_driver_node.cpp
index 39c7411f0130df03d94a921965beaded3333c77b..2c1814aa18ceef6b352fdb5dca43db22f8d31d1f 100644
--- a/driver_templates/template_driver_node.cpp
+++ b/driver_templates/template_driver_node.cpp
@@ -28,7 +28,7 @@ void TemplateNode::mainNodeThread(void)
 {
   //lock access to driver if necessary
   this->driver_.lock();
-  ROS_INFO("TemplateNode::mainNodeThread");
+  ROS_DEBUG("TemplateNode::mainNodeThread");
 
   // [fill msg Header if necessary]
 
diff --git a/driver_templates/template_driver_node.h b/driver_templates/template_driver_node.h
index 2b81b8f54eb04a2de1269b92d7b44da91b7d0552..a68cf5456883dec8330b78d12c484c9f9f7fc806 100644
--- a/driver_templates/template_driver_node.h
+++ b/driver_templates/template_driver_node.h
@@ -96,6 +96,14 @@ class TemplateNode : public iri_base_driver::IriBaseNodeDriver<TemplateDriver>
     * \param nh a reference to the node handle object to manage all ROS topics.
     */
     TemplateNode(ros::NodeHandle& nh);
+    
+    /**
+    * \brief config variable
+    *
+    * This variable has all the driver parameters defined in the cfg config file.
+    * Is updated everytime function config_update() is called.
+    */
+    Config config_;
 
    /**
     * \brief Destructor
diff --git a/libraries/create_action_client.sh b/libraries/create_action_client.sh
index a6ed38f571cdd17abe0f0962038ddc7a85372786..dd857e8ce02971dc1a813a2223cb1aa895a5a2e3 100755
--- a/libraries/create_action_client.sh
+++ b/libraries/create_action_client.sh
@@ -4,10 +4,10 @@
 scripts_path=`echo "${IRI_ROS_SCRIPTS_PATH}"`
 if [[ -z "${scripts_path}" ]]
 then
-  echo "The scripts path environment variable has not been defined. Please see the wiki documentation for instructions on how to create it."
+  echo "ERROR: The scripts path environment variable has not been defined. Please see the wiki documentation for instructions on how to create it."
   exit
-else
-  echo "The scripts path environment variable has been properly defined."
+# else
+#   echo "The scripts path environment variable has been properly defined."
 fi
 
 source "${IRI_ROS_SCRIPTS_PATH}/libraries/scripts_library.sh"
@@ -23,13 +23,13 @@ function check_action_client_file_integrity
   find_comment_in_file "${comment}" "${node_h}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[action server client headers\] from the header file ${node_h}"
+    kill_exit "ERROR: Missing \[action server client headers\] from the header file ${node_h}"
   fi
   comment="\[action client attributes\]"
   find_comment_in_file "${comment}" "${node_h}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[action client attributes\] from the header file ${node_h}"
+    kill_exit "ERROR: Missing \[action client attributes\] from the header file ${node_h}"
   fi
 
   # check node.cpp file
@@ -37,21 +37,21 @@ function check_action_client_file_integrity
   find_comment_in_file "${comment}" "${node_c}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[fill action structure and make request to the action server\] from the source file ${node_c}"
+    kill_exit "ERROR: Missing \[fill action structure and make request to the action server\] from the source file ${node_c}"
   fi
 
   comment="\[action callbacks\]"
   find_comment_in_file "${comment}" "${node_c}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[action callbacks\] from the source file ${node_c}"
+    kill_exit "ERROR: Missing \[action callbacks\] from the source file ${node_c}"
   fi
 
   comment="\[action requests\]"
   find_comment_in_file "${comment}" "${node_c}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[action requests\] from the source file ${node_c}"
+    kill_exit "ERROR: Missing \[action requests\] from the source file ${node_c}"
   fi
 }
 
@@ -72,42 +72,42 @@ function check_action_client_attributes_functions
   find_comment_in_file "${aux_line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "An ActionClient variable already exists with the same name in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: An ActionClient variable already exists with the same name in file ${node_h} line ${line_number}"
   fi
 
   aux_line="${act_pkg}::${action_file}Goal ${act_goal};"
   find_comment_in_file "${aux_line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "An ActionClient goal message variable already exists with the same name in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: An ActionClient goal message variable already exists with the same name in file ${node_h} line ${line_number}"
   fi
 
   aux_line="bool ${topic_name}MakeActionRequest();"
   find_comment_in_file "${aux_line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A MakeActionRequest function already exists with the same name in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: A MakeActionRequest function already exists with the same name in file ${node_h} line ${line_number}"
   fi
 
   aux_line="void ${topic_name}Done(const actionlib::SimpleClientGoalState& state,  const ${act_pkg}::${action_file}ResultConstPtr& result);"
   find_comment_in_file "${aux_line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A Done callback function already exists with the same name in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: A Done callback function already exists with the same name in file ${node_h} line ${line_number}"
   fi
 
   aux_line="void ${topic_name}Active();"
   find_comment_in_file "${aux_line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "An Active callback function already exists with the same name in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: An Active callback function already exists with the same name in file ${node_h} line ${line_number}"
   fi
 
   aux_line="void ${topic_name}Feedback(const ${act_pkg}::${action_file}FeedbackConstPtr& feedback);"
   find_comment_in_file "${aux_line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A Feedback callback function already exists with the same name in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: A Feedback callback function already exists with the same name in file ${node_h} line ${line_number}"
   fi
 
   # check the node.c file
@@ -115,35 +115,35 @@ function check_action_client_attributes_functions
   find_comment_in_file "${aux_line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "The constructor for the Action client is already called in file ${node_c} line ${line_number}"
+    kill_exit "ERROR: The constructor for the Action client is already called in file ${node_c} line ${line_number}"
   fi
 
   aux_line="void ${class_name}::${topic_name}Done(const actionlib::SimpleClientGoalState& state,  const ${act_pkg}::${action_file}ResultConstPtr& result)"
   find_comment_in_file "${aux_line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "An implementation for the Done callback function is already present in file ${node_c} line ${line_number}"
+    kill_exit "ERROR: An implementation for the Done callback function is already present in file ${node_c} line ${line_number}"
   fi
 
   aux_line="void ${class_name}::${topic_name}Active()"
   find_comment_in_file "${aux_line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "An implementation for the Active callback function is already present in file ${node_c} line ${line_number}"
+    kill_exit "ERROR: An implementation for the Active callback function is already present in file ${node_c} line ${line_number}"
   fi
 
   aux_line="void ${class_name}::${topic_name}Feedback(const ${act_pkg}::${action_file}FeedbackConstPtr& feedback)"
   find_comment_in_file "${aux_line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "An implementation for the Feedback callback function is already present in file ${node_c} line ${line_number}"
+    kill_exit "ERROR: An implementation for the Feedback callback function is already present in file ${node_c} line ${line_number}"
   fi
 
   aux_line="bool ${class_name}::${topic_name}MakeActionRequest()"
   find_comment_in_file "${aux_line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "An implementation for the MakeActionRequest function is already present in file ${node_c} line ${line_number}"
+    kill_exit "ERROR: An implementation for the MakeActionRequest function is already present in file ${node_c} line ${line_number}"
   fi
 }
 
@@ -170,7 +170,7 @@ function create_action_client
   get_class_basename "${node_c}"
   if [[ -z ${class_name} ]]
   then
-    kill_exit "impossible to retrieve class basename"
+    kill_exit "ERROR: impossible to retrieve class basename"
   fi
   # echo "class_name=${class_name}"
 
diff --git a/libraries/create_action_server.sh b/libraries/create_action_server.sh
index ccb4be139ceeb0a068cd68d31f9831abc1b5a5df..ec94ef2881006f00601a06d9b28b493efd7968dd 100755
--- a/libraries/create_action_server.sh
+++ b/libraries/create_action_server.sh
@@ -4,10 +4,10 @@
 scripts_path=`echo "${IRI_ROS_SCRIPTS_PATH}"`
 if [[ -z "${scripts_path}" ]]
 then
-  echo "The scripts path environment varibale has not been defined. Please see the wiki documentation for instructions on how to create it."
+  echo "ERROR: The scripts path environment varibale has not been defined. Please see the wiki documentation for instructions on how to create it."
   exit
-else
-  echo "The scripts path environment variable has been properly defined."
+# else
+#   echo "The scripts path environment variable has been properly defined."
 fi
 
 source "${IRI_ROS_SCRIPTS_PATH}/libraries/scripts_library.sh"
@@ -23,13 +23,13 @@ function check_action_server_files_integrity
   find_comment_in_file "${comment}" "${node_h}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[action server client headers\] from the header file ${node_h}"
+    kill_exit "ERROR: Missing \[action server client headers\] from the header file ${node_h}"
   fi
   comment="\[action server attributes\]" 
   find_comment_in_file "${comment}" "${node_h}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[action server attributes\] from the header file ${node_h}"
+    kill_exit "ERROR: Missing \[action server attributes\] from the header file ${node_h}"
   fi
 
   # check the node.cpp file
@@ -37,19 +37,19 @@ function check_action_server_files_integrity
   find_comment_in_file "${comment}" "${node_c}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[init action servers\] from the source file ${node_c}"
+    kill_exit "ERROR: Missing \[init action servers\] from the source file ${node_c}"
   fi
   comment="\[action callbacks\]"
   find_comment_in_file "${comment}" "${node_c}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[action callbacks\] from the source file ${node_c}"
+    kill_exit "ERROR: Missing \[action callbacks\] from the source file ${node_c}"
   fi
   comment="\[fill action structure and make request to the action server\]"
   find_comment_in_file "${comment}" "${node_c}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[fill action structure and make request to the action server\] from the source file ${node_c}"
+    kill_exit "ERROR: Missing \[fill action structure and make request to the action server\] from the source file ${node_c}"
   fi
 }
 
@@ -77,67 +77,67 @@ function check_action_server_attributes_functions
   find_comment_in_file "${aux_line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "An ActionServer variable already exists with the same name in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: An ActionServer variable already exists with the same name in file ${node_h} line ${line_number}"
   fi
 
   aux_line="void ${act_start}(const ${act_pkg}::${action_file}GoalConstPtr& goal);"
   find_comment_in_file "${aux_line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A start callback function already exists with the same name in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: A start callback function already exists with the same name in file ${node_h} line ${line_number}"
   fi
 
   aux_line="void ${act_stop}(void);"
   find_comment_in_file "${aux_line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A stop callback function already exists with the same name in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: A stop callback function already exists with the same name in file ${node_h} line ${line_number}"
   fi
 
   aux_line="bool ${act_finish}(void);"
   find_comment_in_file "${aux_line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "An is finished callback function already exists with the same name in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: An is finished callback function already exists with the same name in file ${node_h} line ${line_number}"
   fi
 
   aux_line="bool ${act_succeed}(void);"
   find_comment_in_file "${aux_line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A has succeeded callback function already exists with the same name in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: A has succeeded callback function already exists with the same name in file ${node_h} line ${line_number}"
   fi
 
   aux_line="void ${act_result}(${act_pkg}::${action_file}ResultPtr& result);"
   find_comment_in_file "${aux_line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A get result callback function already exists with the same name in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: A get result callback function already exists with the same name in file ${node_h} line ${line_number}"
   fi
 
   aux_line="void ${act_feedback}(${act_pkg}::${action_file}FeedbackPtr& feedback);"
   find_comment_in_file "${aux_line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A get feedback callback function already exists with the same name in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: A get feedback callback function already exists with the same name in file ${node_h} line ${line_number}"
   fi
   aux_line="bool ${topic_name}_active;"
   find_comment_in_file "${aux_line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "An active boolean already exists with the same name in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: An active boolean already exists with the same name in file ${node_h} line ${line_number}"
   fi
   aux_line="bool ${topic_name}_succeeded;"
   find_comment_in_file "${aux_line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A success boolean already exists with the same name in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: A success boolean already exists with the same name in file ${node_h} line ${line_number}"
   fi
   aux_line="bool ${topic_name}_finished;"
   find_comment_in_file "${aux_line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A finished boolean already exists with the same name in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: A finished boolean already exists with the same name in file ${node_h} line ${line_number}"
   fi
 
   # check the node.cpp file
@@ -145,119 +145,119 @@ function check_action_server_attributes_functions
   find_comment_in_file "${aux_line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "The constructor for the Action server is already called in file ${node_c} line ${line_number}" 
+    kill_exit "ERROR: The constructor for the Action server is already called in file ${node_c} line ${line_number}" 
   fi
 
   aux_line="${server_name}.registerStartCallback"
   find_comment_in_file "${aux_line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "The registration of the start callback function is already present in file ${node_c} line ${line_number}" 
+    kill_exit "ERROR: The registration of the start callback function is already present in file ${node_c} line ${line_number}" 
   fi
 
   aux_line="${server_name}.registerStopCallback"
   find_comment_in_file "${aux_line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "The registration of the stop callback function is already present in file ${node_c} line ${line_number}" 
+    kill_exit "ERROR: The registration of the stop callback function is already present in file ${node_c} line ${line_number}" 
   fi
 
   aux_line="${server_name}.registerIsFinishedCallback"
   find_comment_in_file "${aux_line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "The registration of the is finished callback function is already present in file ${node_c} line ${line_number}" 
+    kill_exit "ERROR: The registration of the is finished callback function is already present in file ${node_c} line ${line_number}" 
   fi
 
   aux_line="${server_name}.registerHasSucceedCallback"
   find_comment_in_file "${aux_line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "The registration of the has succeeded callback function is already present in file ${node_c} line ${line_number}" 
+    kill_exit "ERROR: The registration of the has succeeded callback function is already present in file ${node_c} line ${line_number}" 
   fi
 
   aux_line="${server_name}.registerGetResultCallback"
   find_comment_in_file "${aux_line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "The registration of the get result callback function is already present in file ${node_c} line ${line_number}" 
+    kill_exit "ERROR: The registration of the get result callback function is already present in file ${node_c} line ${line_number}" 
   fi
 
   aux_line="${server_name}.registerGetFeedbackCallback"
   find_comment_in_file "${aux_line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "The registration of the get feedback callback function is already present in file ${node_c} line ${line_number}" 
+    kill_exit "ERROR: The registration of the get feedback callback function is already present in file ${node_c} line ${line_number}" 
   fi
 
   aux_line="${server_name}.start();"
   find_comment_in_file "${aux_line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "The activation of the action server is already done in file ${node_c} line ${line_number}"
+    kill_exit "ERROR: The activation of the action server is already done in file ${node_c} line ${line_number}"
   fi
 
   line="void ${class_name}::${act_start}(const ${act_pkg}::${action_file}GoalConstPtr& goal)"
   find_comment_in_file "${line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "The implementation of the start callback function is already present in file ${node_c} line ${line_number}" 
+    kill_exit "ERROR: The implementation of the start callback function is already present in file ${node_c} line ${line_number}" 
   fi
 
   line="void ${class_name}::${act_stop}(void)"
   find_comment_in_file "${line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "The implementation of the stop callback function is already present in file ${node_c} line ${line_number}" 
+    kill_exit "ERROR: The implementation of the stop callback function is already present in file ${node_c} line ${line_number}" 
   fi
 
   line="bool ${class_name}::${act_finish}(void)"
   find_comment_in_file "${line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "The implementation of the is finished callback function is already present in file ${node_c} line ${line_number}" 
+    kill_exit "ERROR: The implementation of the is finished callback function is already present in file ${node_c} line ${line_number}" 
   fi
 
   line="bool ${class_name}::${act_succeed}(void)"
   find_comment_in_file "${line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "The implementation of the has succeeded callback function is already present in file ${node_c} line ${line_number}" 
+    kill_exit "ERROR: The implementation of the has succeeded callback function is already present in file ${node_c} line ${line_number}" 
   fi
 
   line="void ${class_name}::${act_result}(${act_pkg}::${action_file}ResultPtr& result)"
   find_comment_in_file "${line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "The implementation of the get result callback function is already present in file ${node_c} line ${line_number}" 
+    kill_exit "ERROR: The implementation of the get result callback function is already present in file ${node_c} line ${line_number}" 
   fi
 
   line="void ${class_name}::${act_feedback}(${act_pkg}::${action_file}FeedbackPtr& feedback)"
   find_comment_in_file "${line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "The implementation of the get feedback callback function is already present in file ${node_c} line ${line_number}" 
+    kill_exit "ERROR: The implementation of the get feedback callback function is already present in file ${node_c} line ${line_number}" 
   fi
 
   aux_line="this->${topic_name}_active=false;"
   find_comment_in_file "${aux_line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "The active variable of the action server is already initialized in file ${node_c} line ${line_number}"
+    kill_exit "ERROR: The active variable of the action server is already initialized in file ${node_c} line ${line_number}"
   fi
 
   aux_line="this->${topic_name}_succeeded=false;"
   find_comment_in_file "${aux_line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "The success variable of the action server is already initialized in file ${node_c} line ${line_number}"
+    kill_exit "ERROR: The success variable of the action server is already initialized in file ${node_c} line ${line_number}"
   fi
 
   aux_line="this->${topic_name}_finished=false;"
   find_comment_in_file "${aux_line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "The finished variable of the action server is already initialized in file ${node_c} line ${line_number}"
+    kill_exit "ERROR: The finished variable of the action server is already initialized in file ${node_c} line ${line_number}"
   fi
 
 }
@@ -291,7 +291,7 @@ function create_action_server
   get_class_basename "${node_c}"
   if [[ -z ${class_name} ]]
   then
-    kill_exit "impossible to retrieve class basename"
+    kill_exit "ERROR: impossible to retrieve class basename"
   fi
   # echo "class_name=${class_name}"
 
diff --git a/libraries/create_client.sh b/libraries/create_client.sh
index dd8f10049700b1cd3069243d0f7f7a929b0e90de..3d6a1add634115c14d8c2ecb8df54484f1004bf2 100755
--- a/libraries/create_client.sh
+++ b/libraries/create_client.sh
@@ -4,10 +4,10 @@
 scripts_path=`echo "${IRI_ROS_SCRIPTS_PATH}"`
 if [[ -z "${scripts_path}" ]]
 then
-  echo "The scripts path environment varibale has not been defined. Please see the wiki documentation for instructions on how to create it."
+  echo "ERROR: The scripts path environment varibale has not been defined. Please see the wiki documentation for instructions on how to create it."
   exit
-else
-  echo "The scripts path environment variable has been properly defined."
+# else
+#   echo "The scripts path environment variable has been properly defined."
 fi
 
 source "${IRI_ROS_SCRIPTS_PATH}/libraries/scripts_library.sh"
@@ -23,26 +23,26 @@ function check_client_file_integrity
   find_comment_in_file "${comment}" "${node_h}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[service client headers\] from the header file ${node_h}"
+    kill_exit "ERROR: Missing \[service client headers\] from the header file ${node_h}"
   fi
   comment="\[client attributes\]"
   find_comment_in_file "${comment}" "${node_h}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[client attributes\] from the header file ${node_h}"
+    kill_exit "ERROR: Missing \[client attributes\] from the header file ${node_h}"
   fi
   # chekc the node.cpp file
   comment="\[init clients\]"
   find_comment_in_file "${comment}" "${node_c}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[init clients\] from the header file ${node_c}"
+    kill_exit "ERROR: Missing \[init clients\] from the header file ${node_c}"
   fi
   comment="\[fill srv structure and make request to the server\]"
   find_comment_in_file "${comment}" "${node_c}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[fill srv structure and make request to the server\] from the header file ${node_c}"
+    kill_exit "ERROR: Missing \[fill srv structure and make request to the server\] from the header file ${node_c}"
   fi
 }
 
@@ -61,13 +61,13 @@ function check_client_attributes_functions
   find_comment_in_file "${line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A service client with the same name is already declared in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: A service client with the same name is already declared in file ${node_h} line ${line_number}"
   fi
   line="${srv_pkg}::${srv_file} ${topic_name}_srv_;"
   find_comment_in_file "${line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A service message with the same name is already declared in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: A service message with the same name is already declared in file ${node_h} line ${line_number}"
   fi
 
   # check the node.cpp file
@@ -76,13 +76,13 @@ function check_client_attributes_functions
   find_comment_in_file "${line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A service client with the same name is already initialized in file ${node_c} line ${line_number}"
+    kill_exit "ERROR: A service client with the same name is already initialized in file ${node_c} line ${line_number}"
   fi
   line="if (${client_name}.call(${topic_name}_srv_))"
   find_comment_in_file "${line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A service with the same name is already called in file ${node_c} line ${line_number}"
+    kill_exit "ERROR: A service with the same name is already called in file ${node_c} line ${line_number}"
   fi
 }
 
@@ -105,7 +105,7 @@ function create_client
   get_class_basename "${node_c}"
   if [[ -z ${class_name} ]]
   then
-    kill_exit "impossible to retrieve class basename"
+    kill_exit "ERROR: impossible to retrieve class basename"
   fi
 # echo "class_name=${class_name}"
 
diff --git a/libraries/create_publisher.sh b/libraries/create_publisher.sh
index 39645fccec23367c08dfdb58f0cb26eb7fce1856..437972a2bac2e85858e7345f680d3f7c4eb8d87e 100755
--- a/libraries/create_publisher.sh
+++ b/libraries/create_publisher.sh
@@ -4,10 +4,10 @@
 scripts_path=`echo "${IRI_ROS_SCRIPTS_PATH}"`
 if [[ -z "${scripts_path}" ]]
 then
-  echo "The scripts path environment variable has not been defined. Please see the wiki documentation for instructions on how to create it."
+  echo "ERROR: The scripts path environment variable has not been defined. Please see the wiki documentation for instructions on how to create it."
   exit
-else
-  echo "The scripts path environment variable has been properly defined."
+# else
+#   echo "The scripts path environment variable has been properly defined."
 fi
 
 source "${IRI_ROS_SCRIPTS_PATH}/libraries/scripts_library.sh"
@@ -23,13 +23,13 @@ function check_publisher_file_integrity
   find_comment_in_file "${comment}" "${node_h}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[publisher subscriber headers\] from the header file ${node_h}"
+    kill_exit "ERROR: Missing \[publisher subscriber headers\] from the header file ${node_h}"
   fi
   comment="\[publisher attributes\]"
   find_comment_in_file "${comment}" "${node_h}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[publisher attributes\] from the header file ${node_h}"
+    kill_exit "ERROR: Missing \[publisher attributes\] from the header file ${node_h}"
   fi
 
   # check the node.cpp file
@@ -37,20 +37,20 @@ function check_publisher_file_integrity
   find_comment_in_file "${comment}" "${node_c}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[init publishers\] from the header file ${node_c}"
+    kill_exit "ERROR: Missing \[init publishers\] from the header file ${node_c}"
   fi
   comment="\[fill msg structures\]"
   find_comment_in_file "${comment}" "${node_c}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[fill msg structures\] from the header file ${node_c}"
+    kill_exit "ERROR: Missing \[fill msg structures\] from the header file ${node_c}"
   fi
 
   comment="\[publish messages\]"
   find_comment_in_file "${comment}" "${node_c}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[publish messages\] from the header file ${node_c}"
+    kill_exit "ERROR: Missing \[publish messages\] from the header file ${node_c}"
   fi
 }
 
@@ -72,20 +72,20 @@ function check_publisher_attributes_functions
     find_comment_in_file "${line}" "${node_h}"
     if [[ "${comment_found}" = "true" ]]
     then
-      kill_exit "A publisher with the same name is already declared in file ${node_h} line ${line_number}"
+      kill_exit "ERROR: A publisher with the same name is already declared in file ${node_h} line ${line_number}"
     fi
     line="camera_info_manager::CameraInfoManager ${topic_name}_camera_manager;"
     find_comment_in_file "${line}" "${node_h}"
     if [[ "${comment_found}" = "true" ]]
     then
-      kill_exit "A camera manager with the same name is already declared in file ${node_h} line ${line_number}"
+      kill_exit "ERROR: A camera manager with the same name is already declared in file ${node_h} line ${line_number}"
     fi
   else
     line="ros::Publisher ${publisher_name};"
     find_comment_in_file "${line}" "${node_h}"
     if [[ "${comment_found}" = "true" ]]
     then
-      kill_exit "A publisher with the same name is already declared in file ${node_h} line ${line_number}"
+      kill_exit "ERROR: A publisher with the same name is already declared in file ${node_h} line ${line_number}"
     fi
   fi
 
@@ -93,7 +93,7 @@ function check_publisher_attributes_functions
   find_comment_in_file "${line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A message variable with the same name is already declared in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: A message variable with the same name is already declared in file ${node_h} line ${line_number}"
   fi
 
   # check the node.cpp file
@@ -103,50 +103,50 @@ function check_publisher_attributes_functions
     find_comment_in_file "${line}" "${node_c}"
     if [[ "${comment_found}" = "true" ]]
     then
-      kill_exit "A camera manager with the same name is already initialized in file ${node_c} line ${line_number}"
+      kill_exit "ERROR: A camera manager with the same name is already initialized in file ${node_c} line ${line_number}"
     fi
     line="this->${publisher_name} = this->it.advertiseCamera(\"${topic_name}/image_raw\", ${buffer});"
     find_comment_in_file "${line}" "${node_c}"
     if [[ "${comment_found}" = "true" ]]
     then
-      kill_exit "A topic with the same name is already advertised in file ${node_c} line ${line_number}"
+      kill_exit "ERROR: A topic with the same name is already advertised in file ${node_c} line ${line_number}"
     fi
     line="this->${topic_name}_camera_manager.validateURL(${topic_name}_cal_file)"
     find_comment_in_file "${line}" "${node_c}"
     if [[ "${comment_found}" = "true" ]]
     then
-      kill_exit "The calibration file URL for a camera manager with the same name is already validated in file ${node_c} line ${line_number}"
+      kill_exit "ERROR: The calibration file URL for a camera manager with the same name is already validated in file ${node_c} line ${line_number}"
     fi
     line="this->${topic_name}_camera_manager.loadCameraInfo(${topic_name}_cal_file)"
     find_comment_in_file "${line}" "${node_c}"
     if [[ "${comment_found}" = "true" ]]
     then
-      kill_exit "The camera information for a camera manager with the same name is already loaded in file ${node_c} line ${line_number}"
+      kill_exit "ERROR: The camera information for a camera manager with the same name is already loaded in file ${node_c} line ${line_number}"
     fi
     line="sensor_msgs::CameraInfo ${topic_name}_camera_info=this->${topic_name}_camera_manager.getCameraInfo();"
     find_comment_in_file "${line}" "${node_c}"
     if [[ "${comment_found}" = "true" ]]
     then
-      kill_exit "The camera info structure for a topic with the same name is already initialized in file ${node_c} line ${line_number}"
+      kill_exit "ERROR: The camera info structure for a topic with the same name is already initialized in file ${node_c} line ${line_number}"
     fi
     line="//this->${publisher_name}.publish(this->${topic_name}_${msg_file}_msg_,${topic_name}_camera_info);"
     find_comment_in_file "${line}" "${node_c}"
     if [[ "${comment_found}" = "true" ]]
     then
-      kill_exit "A message with the same name is already published in file ${node_c} line ${line_number}"
+      kill_exit "ERROR: A message with the same name is already published in file ${node_c} line ${line_number}"
     fi
   else
     line="this->${publisher_name} = this->private_node_handle_.advertise<${msg_pkg}::${msg_file}>(\"${topic_name}\", ${buffer});"
     find_comment_in_file "${line}" "${node_c}"
     if [[ "${comment_found}" = "true" ]]
     then
-      kill_exit "A publisher with the same name is already advertised in file ${node_c} line ${line_number}"
+      kill_exit "ERROR: A publisher with the same name is already advertised in file ${node_c} line ${line_number}"
     fi
     line="this->${publisher_name}.publish(this->${topic_name}_${msg_file}_msg_);"
     find_comment_in_file "${line}" "${node_c}"
     if [[ "${comment_found}" = "true" ]]
     then
-      kill_exit "A message with the same name is already published in file ${node_c} line ${line_number}"
+      kill_exit "ERROR: A message with the same name is already published in file ${node_c} line ${line_number}"
     fi
   fi
 }
@@ -174,7 +174,7 @@ function create_publisher
   get_class_basename "${node_c}"
   if [[ -z ${class_name} ]]
   then
-    kill_exit "impossible to retrieve class basename"
+    kill_exit "ERROR: impossible to retrieve class basename"
   fi
 
   #check files integrity before making any changes
diff --git a/libraries/create_server.sh b/libraries/create_server.sh
index fc7782fec1d81180182ebf9817134c0afa21d23b..5eb7db04047a34d80175fc1ed93ee2f2bd797683 100755
--- a/libraries/create_server.sh
+++ b/libraries/create_server.sh
@@ -4,10 +4,10 @@
 scripts_path=`echo "${IRI_ROS_SCRIPTS_PATH}"`
 if [[ -z "${scripts_path}" ]]
 then
-  echo "The scripts path environment variable has not been defined. Please see the wiki documentation for instructions on how to create it."
+  echo "ERROR: The scripts path environment variable has not been defined. Please see the wiki documentation for instructions on how to create it."
   exit
-else
-  echo "The scripts path environment variable has been properly defined."
+# else
+#   echo "The scripts path environment variable has been properly defined."
 fi
 
 source "${IRI_ROS_SCRIPTS_PATH}/libraries/scripts_library.sh"
@@ -23,13 +23,13 @@ function check_server_file_integrity
   find_comment_in_file "${comment}" "${node_h}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[service client headers\] from the header file ${node_h}"
+    kill_exit "ERROR: Missing \[service client headers\] from the header file ${node_h}"
   fi
   comment="\[service attributes\]"
   find_comment_in_file "${comment}" "${node_h}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[service attributes\] from the header file ${node_h}"
+    kill_exit "ERROR: Missing \[service attributes\] from the header file ${node_h}"
   fi
 
   # check the node.cpp file
@@ -37,19 +37,19 @@ function check_server_file_integrity
   find_comment_in_file "${comment}" "${node_c}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[init services\] from the header file ${node_c}"
+    kill_exit "ERROR: Missing \[init services\] from the header file ${node_c}"
   fi 
   comment="\[service callbacks\]"
   find_comment_in_file "${comment}" "${node_c}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[service callbacks\] from the header file ${node_c}"
+    kill_exit "ERROR: Missing \[service callbacks\] from the header file ${node_c}"
   fi 
   comment="\[free dynamic memory\]"
   find_comment_in_file "${comment}" "${node_c}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[free dynamic memory\] from the header file ${node_c}"
+    kill_exit "ERROR: Missing \[free dynamic memory\] from the header file ${node_c}"
   fi 
 }
 
@@ -71,31 +71,31 @@ function check_server_attributes_functions
   find_comment_in_file "${line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A service server with the same name is already declared in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: A service server with the same name is already declared in file ${node_h} line ${line_number}"
   fi
   line="bool ${callback}(${srv_pkg}::${srv_file}::Request &req, ${srv_pkg}::${srv_file}::Response &res);"
   find_comment_in_file "${line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A service callback function with the same name is already declared in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: A service callback function with the same name is already declared in file ${node_h} line ${line_number}"
   fi
   line="pthread_mutex_t ${mutex_name};"
   find_comment_in_file "${line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A mutex with the same name is already declared in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: A mutex with the same name is already declared in file ${node_h} line ${line_number}"
   fi
   line="void ${mutex_name}enter(void);"
   find_comment_in_file "${line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A mutex enter function with the same name is already declared in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: A mutex enter function with the same name is already declared in file ${node_h} line ${line_number}"
   fi
   line="void ${mutex_name}exit(void);"
   find_comment_in_file "${line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A mutex exit function with the same name is already declared in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: A mutex exit function with the same name is already declared in file ${node_h} line ${line_number}"
   fi
 
   # check the node.cpp file
@@ -103,37 +103,37 @@ function check_server_attributes_functions
   find_comment_in_file "${line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A service server with the same name is already initialized in file ${node_c} line ${line_number}"
+    kill_exit "ERROR: A service server with the same name is already initialized in file ${node_c} line ${line_number}"
   fi
   line="pthread_mutex_init(&this->${mutex_name},NULL);"
   find_comment_in_file "${line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A mutex with the same name is already initialized in file ${node_c} line ${line_number}"
+    kill_exit "ERROR: A mutex with the same name is already initialized in file ${node_c} line ${line_number}"
   fi
   line="pthread_mutex_destroy(&this->${mutex_name});"
   find_comment_in_file "${line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A mutex with the same name is already destroyed in file ${node_c} line ${line_number}"
+    kill_exit "ERROR: A mutex with the same name is already destroyed in file ${node_c} line ${line_number}"
   fi
   aux_line="bool ${class_name}::${callback}(${srv_pkg}::${srv_file}::Request &req, ${srv_pkg}::${srv_file}::Response &res)"
   find_comment_in_file "${aux_line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A service callback function with the same name is already implemented in file ${node_c} line ${line_number}"
+    kill_exit "ERROR: A service callback function with the same name is already implemented in file ${node_c} line ${line_number}"
   fi
   line="void ${class_name}::${mutex_name}enter(void)"
   find_comment_in_file "${line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A mutex enter function  with the same name is already implemented in file ${node_c} line ${line_number}"
+    kill_exit "ERROR: A mutex enter function  with the same name is already implemented in file ${node_c} line ${line_number}"
   fi
   line="void ${class_name}::${mutex_name}exit(void)"
   find_comment_in_file "${line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A mutex exit function  with the same name is already implemented in file ${node_c} line ${line_number}"
+    kill_exit "ERROR: A mutex exit function  with the same name is already implemented in file ${node_c} line ${line_number}"
   fi
 }
 
@@ -158,7 +158,7 @@ function create_server
   get_class_basename "${node_c}"
   if [[ -z ${class_name} ]]
   then
-    kill_exit "impossible to retrieve class basename"
+    kill_exit "ERROR: impossible to retrieve class basename"
   fi
 # echo "class_name=${class_name}"
 
diff --git a/libraries/create_subscriber.sh b/libraries/create_subscriber.sh
index 892ad25face028db6f2443568c65159857268a7e..1ebb5aef7baf10bd7e7aa230888632690247a3c4 100755
--- a/libraries/create_subscriber.sh
+++ b/libraries/create_subscriber.sh
@@ -4,10 +4,10 @@
 scripts_path=`echo "${IRI_ROS_SCRIPTS_PATH}"`
 if [[ -z "${scripts_path}" ]]
 then
-  echo "The scripts path environment variable has not been defined. Please see the wiki documentation for instructions on how to create it."
+  echo "ERROR: The scripts path environment variable has not been defined. Please see the wiki documentation for instructions on how to create it."
   exit
-else
-  echo "The scripts path environment variable has been properly defined."
+# else
+#   echo "The scripts path environment variable has been properly defined."
 fi
 
 source "${IRI_ROS_SCRIPTS_PATH}/libraries/scripts_library.sh"
@@ -23,13 +23,13 @@ function check_subscriber_file_integrity
   find_comment_in_file "${comment}" "${node_h}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[publisher subscriber headers\] from the header file ${node_h}"
+    kill_exit "ERROR: Missing \[publisher subscriber headers\] from the header file ${node_h}"
   fi
   comment="\[subscriber attributes\]"
   find_comment_in_file "${comment}" "${node_h}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[subscriber attributes\] from the header file ${node_h}"
+    kill_exit "ERROR: Missing \[subscriber attributes\] from the header file ${node_h}"
   fi
 
   # check node.cpp file
@@ -37,19 +37,19 @@ function check_subscriber_file_integrity
   find_comment_in_file "${comment}" "${node_c}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[init subscribers\] from the header file ${node_c}"
+    kill_exit "ERROR: Missing \[init subscribers\] from the header file ${node_c}"
   fi
   comment="\[subscriber callbacks\]"
   find_comment_in_file "${comment}" "${node_c}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[subscriber callbacks\] from the header file ${node_c}"
+    kill_exit "ERROR: Missing \[subscriber callbacks\] from the header file ${node_c}"
   fi
   comment="\[free dynamic memory\]"
   find_comment_in_file "${comment}" "${node_c}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[free dynamic memory\] from the header file ${node_c}"
+    kill_exit "ERROR: Missing \[free dynamic memory\] from the header file ${node_c}"
   fi
 }
 
@@ -74,27 +74,27 @@ function check_subscriber_attributes_functions
     find_comment_in_file "${line}" "${node_h}"
     if [[ "${comment_found}" = "true" ]]
     then
-      kill_exit "A subscriber with the same name is already declared in file ${node_h} line ${line_number}"
+      kill_exit "ERROR: A subscriber with the same name is already declared in file ${node_h} line ${line_number}"
     fi
     line="void ${callback}(const ${msg_pkg}::${msg_file}::ConstPtr& msg,const sensor_msgs::CameraInfoConstPtr& info);"
     find_comment_in_file "${line}" "${node_h}"
     if [[ "${comment_found}" = "true" ]]
     then
-      kill_exit "A subscriber callback function with the same name is already declared in file ${node_h} line ${line_number}"
+      kill_exit "ERROR: A subscriber callback function with the same name is already declared in file ${node_h} line ${line_number}"
     fi
   else
     line="ros::Subscriber ${subscriber_name};"
     find_comment_in_file "${line}" "${node_h}"
     if [[ "${comment_found}" = "true" ]]
     then
-      kill_exit "A subscriber with the same name is already declared in file ${node_h} line ${line_number}"
+      kill_exit "ERROR: A subscriber with the same name is already declared in file ${node_h} line ${line_number}"
     fi
 
     line="void ${callback}(const ${msg_pkg}::${msg_file}::ConstPtr& msg);"
     find_comment_in_file "${line}" "${node_h}"
     if [[ "${comment_found}" = "true" ]]
     then
-      kill_exit "A subscriber callback function with the same name is already declared in file ${node_h} line ${line_number}"
+      kill_exit "ERROR: A subscriber callback function with the same name is already declared in file ${node_h} line ${line_number}"
     fi
   fi
 
@@ -102,21 +102,21 @@ function check_subscriber_attributes_functions
   find_comment_in_file "${line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A mutex with the same name is already declared in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: A mutex with the same name is already declared in file ${node_h} line ${line_number}"
   fi
 
   line="void ${mutex_name}enter(void);"
   find_comment_in_file "${line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A mutex enter function with the same name is already declared in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: A mutex enter function with the same name is already declared in file ${node_h} line ${line_number}"
   fi
 
   line="void ${mutex_name}exit(void);"
   find_comment_in_file "${line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A mutex exit function with the same name is already declared in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: A mutex exit function with the same name is already declared in file ${node_h} line ${line_number}"
   fi
 
   # check node.cpp file
@@ -126,14 +126,14 @@ function check_subscriber_attributes_functions
     find_comment_in_file "${line}" "${node_c}"
     if [[ "${comment_found}" = "true" ]]
     then
-      kill_exit "A subscriber with the same name is already subscribed in file ${node_c} line ${line_number}"
+      kill_exit "ERROR: A subscriber with the same name is already subscribed in file ${node_c} line ${line_number}"
     fi
   else
     line="this->${subscriber_name} = this->private_node_handle_.subscribe(\"${topic_name}\", ${buffer}, &${class_name}::${callback}, this);"
     find_comment_in_file "${line}" "${node_c}"
     if [[ "${comment_found}" = "true" ]]
     then
-      kill_exit "A subscriber with the same name is already subscribed in file ${node_c} line ${line_number}"
+      kill_exit "ERROR: A subscriber with the same name is already subscribed in file ${node_c} line ${line_number}"
     fi
   fi
 
@@ -141,14 +141,14 @@ function check_subscriber_attributes_functions
   find_comment_in_file "${line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A mutex with the same name is already initialized in file ${node_c} line ${line_number}"
+    kill_exit "ERROR: A mutex with the same name is already initialized in file ${node_c} line ${line_number}"
   fi
 
   line="pthread_mutex_destroy(&this->${mutex_name});"
   find_comment_in_file "${line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A mutex with the same name is already destroyed in file ${node_c} line ${line_number}"
+    kill_exit "ERROR: A mutex with the same name is already destroyed in file ${node_c} line ${line_number}"
   fi
 
   #look if callback is already defined in file
@@ -158,27 +158,27 @@ function check_subscriber_attributes_functions
     find_comment_in_file "${aux_line}" "${node_c}"
     if [[ "${comment_found}" = "true" ]]
     then
-      kill_exit "A subscriber callback function with the same name is already implemented in file ${node_c} line ${line_number}"
+      kill_exit "ERROR: A subscriber callback function with the same name is already implemented in file ${node_c} line ${line_number}"
     fi
   else
     aux_line="void ${class_name}::${callback}(const ${msg_pkg}::${msg_file}::ConstPtr& msg)"
     find_comment_in_file "${aux_line}" "${node_c}"
     if [[ "${comment_found}" = "true" ]]
     then
-      kill_exit "A subscriber callback function with the same name is already implemented in file ${node_c} line ${line_number}"
+      kill_exit "ERROR: A subscriber callback function with the same name is already implemented in file ${node_c} line ${line_number}"
     fi
   fi
   line="void ${class_name}::${mutex_name}enter(void)"
   find_comment_in_file "${line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A mutex enter function  with the same name is already implemented in file ${node_c} line ${line_number}"
+    kill_exit "ERROR: A mutex enter function  with the same name is already implemented in file ${node_c} line ${line_number}"
   fi
   line="void ${class_name}::${mutex_name}exit(void)"
   find_comment_in_file "${line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A mutex exit function  with the same name is already implemented in file ${node_c} line ${line_number}"
+    kill_exit "ERROR: A mutex exit function  with the same name is already implemented in file ${node_c} line ${line_number}"
   fi
 }
 
@@ -207,7 +207,7 @@ function create_subscriber
   get_class_basename "${node_c}"
   if [[ -z ${class_name} ]]
   then
-    kill_exit "impossible to retrieve class basename"
+    kill_exit "ERROR: impossible to retrieve class basename"
   fi
 # echo "class_name=${class_name}"
 
diff --git a/libraries/create_tf_broadcaster.sh b/libraries/create_tf_broadcaster.sh
index 19807c3a294a2452cf7a3900d1a3fdd8fa34f1eb..b8992bd1601fdccdf46d0e6893219314fc4052f6 100755
--- a/libraries/create_tf_broadcaster.sh
+++ b/libraries/create_tf_broadcaster.sh
@@ -4,10 +4,10 @@
 scripts_path=`echo "${IRI_ROS_SCRIPTS_PATH}"`
 if [[ -z "${scripts_path}" ]]
 then
-  echo "The scripts path environment variable has not been defined. Please see the wiki documentation for instructions on how to create it."
+  echo "ERROR: The scripts path environment variable has not been defined. Please see the wiki documentation for instructions on how to create it."
   exit
-else
-  echo "The scripts path environment variable has been properly defined."
+# else
+#   echo "The scripts path environment variable has been properly defined."
 fi
 
 source "${IRI_ROS_SCRIPTS_PATH}/libraries/scripts_library.sh"
@@ -23,13 +23,13 @@ function check_publisher_file_integrity
   find_comment_in_file "${comment}" "${node_h}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[publisher subscriber headers\] from the header file ${node_h}"
+    kill_exit "ERROR: Missing \[publisher subscriber headers\] from the header file ${node_h}"
   fi
   comment="\[publisher attributes\]"
   find_comment_in_file "${comment}" "${node_h}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[publisher attributes\] from the header file ${node_h}"
+    kill_exit "ERROR: Missing \[publisher attributes\] from the header file ${node_h}"
   fi
 
   # check the node.cpp file
@@ -37,20 +37,20 @@ function check_publisher_file_integrity
   find_comment_in_file "${comment}" "${node_c}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[init publishers\] from the header file ${node_c}"
+    kill_exit "ERROR: Missing \[init publishers\] from the header file ${node_c}"
   fi
   comment="\[fill msg structures\]"
   find_comment_in_file "${comment}" "${node_c}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[fill msg structures\] from the header file ${node_c}"
+    kill_exit "ERROR: Missing \[fill msg structures\] from the header file ${node_c}"
   fi
 
   comment="\[publish messages\]"
   find_comment_in_file "${comment}" "${node_c}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[publish messages\] from the header file ${node_c}"
+    kill_exit "ERROR: Missing \[publish messages\] from the header file ${node_c}"
   fi
 }
 
@@ -66,14 +66,14 @@ function check_publisher_attributes_functions
   find_comment_in_file "${line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A TF broadcaster with the same name is already declared in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: A TF broadcaster with the same name is already declared in file ${node_h} line ${line_number}"
   fi
 
   line="geometry_msgs::TransformStampd transform_msg_;"
   find_comment_in_file "${line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A message variable with the same name is already declared in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: A message variable with the same name is already declared in file ${node_h} line ${line_number}"
   fi
 
   # check the node.cpp file
@@ -81,7 +81,7 @@ function check_publisher_attributes_functions
   find_comment_in_file "${line}" "${node_c}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A message with the same name is already published in file ${node_c} line ${line_number}"
+    kill_exit "ERROR: A message with the same name is already published in file ${node_c} line ${line_number}"
   fi
 }
 
@@ -104,7 +104,7 @@ function create_tf_broadcaster
   get_class_basename "${node_c}"
   if [[ -z ${class_name} ]]
   then
-    kill_exit "impossible to retrieve class basename"
+    kill_exit "ERROR: impossible to retrieve class basename"
   fi
 
   #check files integrity before making any changes
diff --git a/libraries/create_tf_listener.sh b/libraries/create_tf_listener.sh
index 828e2c3719acdf295e1219171374342781d41d76..88edae8c376390f6eb559469ca01a4fb8cb18144 100644
--- a/libraries/create_tf_listener.sh
+++ b/libraries/create_tf_listener.sh
@@ -4,10 +4,10 @@
 scripts_path=`echo "${IRI_ROS_SCRIPTS_PATH}"`
 if [[ -z "${scripts_path}" ]]
 then
-  echo "The scripts path environment variable has not been defined. Please see the wiki documentation for instructions on how to create it."
+  echo "ERROR: The scripts path environment variable has not been defined. Please see the wiki documentation for instructions on how to create it."
   exit
-else
-  echo "The scripts path environment variable has been properly defined."
+# else
+#   echo "The scripts path environment variable has been properly defined."
 fi
 
 source "${IRI_ROS_SCRIPTS_PATH}/libraries/scripts_library.sh"
@@ -23,13 +23,13 @@ function check_subscriber_file_integrity
   find_comment_in_file "${comment}" "${node_h}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[publisher subscriber headers\] from the header file ${node_h}"
+    kill_exit "ERROR: Missing \[publisher subscriber headers\] from the header file ${node_h}"
   fi
   comment="\[subscriber attributes\]"
   find_comment_in_file "${comment}" "${node_h}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[subscriber attributes\] from the header file ${node_h}"
+    kill_exit "ERROR: Missing \[subscriber attributes\] from the header file ${node_h}"
   fi
 
   # check node.cpp file
@@ -37,19 +37,19 @@ function check_subscriber_file_integrity
   find_comment_in_file "${comment}" "${node_c}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[init subscribers\] from the header file ${node_c}"
+    kill_exit "ERROR: Missing \[init subscribers\] from the header file ${node_c}"
   fi
   comment="\[subscriber callbacks\]"
   find_comment_in_file "${comment}" "${node_c}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[subscriber callbacks\] from the header file ${node_c}"
+    kill_exit "ERROR: Missing \[subscriber callbacks\] from the header file ${node_c}"
   fi
   comment="\[free dynamic memory\]"
   find_comment_in_file "${comment}" "${node_c}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "Missing \[free dynamic memory\] from the header file ${node_c}"
+    kill_exit "ERROR: Missing \[free dynamic memory\] from the header file ${node_c}"
   fi
 }
 
@@ -67,7 +67,7 @@ function check_subscriber_attributes_functions
   find_comment_in_file "${line}" "${node_h}"
   if [[ "${comment_found}" = "true" ]]
   then
-    kill_exit "A TF listener with the same name is already declared in file ${node_h} line ${line_number}"
+    kill_exit "ERROR: A TF listener with the same name is already declared in file ${node_h} line ${line_number}"
   fi
 
   # check node.cpp file
@@ -93,7 +93,7 @@ function create_tf_listener
   get_class_basename "${node_c}"
   if [[ -z ${class_name} ]]
   then
-    kill_exit "impossible to retrieve class basename"
+    kill_exit "ERROR: impossible to retrieve class basename"
   fi
 # echo "class_name=${class_name}"
 
@@ -103,6 +103,14 @@ function create_tf_listener
   check_subscriber_file_integrity "${node_h}" "${node_c}"
   check_subscriber_attributes_functions "${node_h}" "${node_c}" "${listener_name}" "${class_name}"
 
+  local obj=""
+  if [[ "${driver_alg}" = "driver" ]]
+  then
+    obj="this->driver_."
+  else
+    obj="this->alg_."
+  fi
+
 ################################################################################
 # modify Node.h #
 
@@ -124,7 +132,7 @@ function create_tf_listener
   line="\n"
   aux_line="/*"
   line="${line}\ \ ${aux_line}\n"
-  aux_line="//tf_listener example"
+  aux_line="//tf_listener example BEGIN"
   line="${line}\ \ ${aux_line}\n"
   aux_line="try{"
   line="${line}\ \ ${aux_line}\n"
@@ -138,8 +146,16 @@ function create_tf_listener
   line="${line}\ \ ${aux_line}\n"
   aux_line="  ros::Duration polling_sleep_duration = ros::Duration(0.01);"
   line="${line}\ \ ${aux_line}\n"
+  
+  aux_line="  ${obj}unlock();"
+  line="${line}\ \ ${aux_line}\n"
+
   aux_line="  bool tf_exists = this->tf_listener_.waitForTransform(target_frame, source_frame, time, timeout, polling_sleep_duration);"
   line="${line}\ \ ${aux_line}\n"
+  
+  aux_line="  ${obj}lock();"
+  line="${line}\ \ ${aux_line}\n"
+
   aux_line="  if(tf_exists){"
   line="${line}\ \ ${aux_line}\n"
   aux_line="    geometry_msgs::PoseStamped stamped_pose_in;"
@@ -193,7 +209,7 @@ function create_tf_listener
   line="${line}\ \ ${aux_line}\n"
   aux_line="  ROS_ERROR(\"TF Exception: %s\",ex.what()); }"
   line="${line}\ \ ${aux_line}\n"
-  aux_line="///tf_listener example"
+  aux_line="///tf_listener example END"
   line="${line}\ \ ${aux_line}\n"
   aux_line="*/"
   line="${line}\ \ ${aux_line}\n"
diff --git a/libraries/scripts_library.sh b/libraries/scripts_library.sh
index 17dba638ba1cad7e90effff05d9ebd7e212cd275..d96c2621d697a133a63e417f550aaa97999d4e94 100755
--- a/libraries/scripts_library.sh
+++ b/libraries/scripts_library.sh
@@ -1,6 +1,6 @@
 #!/bin/bash
 
-source "${ROS_ROOT}/../rosbash/rosbash" || kill_exit "ROS_ROOT Not found, try to install ROS again"
+source "${ROS_ROOT}/../rosbash/rosbash" || kill_exit "ERROR: ROS_ROOT Not found, try to install ROS again"
 
 # kill_exit
 # prints string in $1 and exits script
@@ -17,21 +17,21 @@ function kill_exit
 # not been found exits the script
 function check_libraries
 {
-  echo "checking libraries ..."
+  echo "Checking libraries ..."
   if [[ -d "${IRI_ROS_SCRIPTS_PATH}/libraries" ]]
   then
-    pushd "${IRI_ROS_SCRIPTS_PATH}/libraries"
+    pushd "${IRI_ROS_SCRIPTS_PATH}/libraries" > /dev/null
     if [[ -e "create_server.sh" ]] && [[ -e "create_client.sh" ]] && [[ -e "create_publisher.sh" ]] && [[ -e "create_subscriber.sh" ]] && [[ -e "create_tf_broadcaster.sh" ]] && [[ -e "create_tf_listener.sh" ]]
     then
       echo "All library files available"
-      popd
+      popd > /dev/null
     else
-      popd
-      kill_exit "Missing some script files, please download IRI_ROS scripts again, aborting ..."
+      popd > /dev/null
+      kill_exit "ERROR: Missing some script files, please download IRI_ROS scripts again, aborting ..."
     fi
   else
-    popd
-    kill_exit "Missing libraries folder, please download IRI_ROS scripts again, aborting ..."
+    popd > /dev/null
+    kill_exit "ERROR: Missing libraries folder, please download IRI_ROS scripts again, aborting ..."
   fi
 }
 
@@ -40,38 +40,38 @@ function check_libraries
 # missing then exits the script
 function check_templates
 {
-  echo "checking driver templates ..."
+  echo "Checking driver templates ..."
   if [[ -d "${IRI_ROS_SCRIPTS_PATH}/driver_templates" ]]
   then
-    pushd "${IRI_ROS_SCRIPTS_PATH}/driver_templates"
+    pushd "${IRI_ROS_SCRIPTS_PATH}/driver_templates" > /dev/null
     if [[ -e "CMakeLists.txt" ]] && [[ -e "template_driver.cfg" ]] && [[ -e "template_driver.h" ]] && [[ -e "template_driver.cpp" ]] && [[ -e "template_driver_node.h" ]] && [[ -e "template_driver_node.cpp" ]]
     then
       echo "Driver template files available"
-      popd
+      popd > /dev/null
     else
-      popd
-      kill_exit "Missing some driver template files, please download IRI_ROS scripts again, aborting ..."
+      popd > /dev/null
+      kill_exit "ERROR: Missing some driver template files, please download IRI_ROS scripts again, aborting ..."
     fi
   else
-    popd
-    kill_exit "Missing driver templates folder, please download IRI_ROS scripts again, aborting ..."
+    popd > /dev/null
+    kill_exit "ERROR: Missing driver templates folder, please download IRI_ROS scripts again, aborting ..."
   fi
 
-  echo "checking algorithm templates ..."
+  echo "Checking algorithm templates ..."
   if [[ -d "${IRI_ROS_SCRIPTS_PATH}/algorithm_templates" ]]
   then
-    pushd "${IRI_ROS_SCRIPTS_PATH}/algorithm_templates"
+    pushd "${IRI_ROS_SCRIPTS_PATH}/algorithm_templates" > /dev/null
     if [[ -e "CMakeLists.txt" ]] && [[ -e "template_alg.cfg" ]] && [[ -e "template_alg.cpp" ]] && [[ -e "template_alg.h" ]] && [[ -e "template_alg_node.cpp" ]] && [[ -e "template_alg_node.h" ]]
     then
       echo "Generic algorithm template files available"
-      popd
+      popd > /dev/null
     else
-      popd
-      kill_exit "Missing some generic algorithm template files, please download IRI_ROS scripts again, aborting ..."
+      popd > /dev/null
+      kill_exit "ERROR: Missing some generic algorithm template files, please download IRI_ROS scripts again, aborting ..."
     fi
   else
-    popd
-    kill_exit "Missing generic algorithm templates folder, please download IRI_ROS scripts again, aborting ..."
+    popd > /dev/null
+    kill_exit "ERROR: Missing generic algorithm templates folder, please download IRI_ROS scripts again, aborting ..."
   fi
 }
 
@@ -82,7 +82,7 @@ function check_package
 {
   if [[ -z $1 ]]
   then
-    kill_exit "check_package: missing input parameters"
+    kill_exit "ERROR: check_package: missing input parameters"
   fi
 
   #save "roscd pkg" output
@@ -104,7 +104,7 @@ function create_basename
 {
   if [[ -z $1 ]]
   then
-    kill_exit "create_basename: missing input parameters"
+    kill_exit "ERROR: create_basename: missing input parameters"
   fi
 
   local in_pkg=$1
@@ -129,7 +129,7 @@ function get_class_basename
 {
   if [[ -z $1 ]]
   then
-    kill_exit "create_basename: missing input parameters"
+    kill_exit "ERROR: get_class_basename missing input parameters"
   fi
 
   local source_file=$1
@@ -147,7 +147,7 @@ function browse_files
 {
   if [[ -z $1 ]] || [[ -z $2 ]]
   then
-    kill_exit "check_package: missing input parameters"
+    kill_exit "ERROR: browse_files missing input parameters"
   fi
 
   local my_file=$1
@@ -183,7 +183,7 @@ function find_ros_message
 {
   if [[ -z $1 ]] || [[ -z $2 ]] || [[ -z $3 ]]
   then
-    kill_exit "find_ros_message: missing input parameters"
+    kill_exit "ERROR: find_ros_message: missing input parameters"
   fi
 
   # read input parameters
@@ -230,7 +230,6 @@ function find_ros_message
       local parsed_name=$(echo ${my_file} | sed 's/\// /g')
       file_pkg=${parsed_name%% *}
       my_file=${parsed_name##* }
-
     else
       file_pkg=
     fi
@@ -238,57 +237,18 @@ function find_ros_message
     if [[ -z ${file_pkg} ]]
     then
       file_pkg=(`echo ${ros_out} | tr '[' ' ' | tr ']' ' ' | tr '/' ' '`)
+      lines_found=`echo "$ros_out" | wc -l`
+      if [[ $lines_found != 1 ]]
+      then
+        echo "WARNING: Found multiple packages ($lines_found) containing the same message filename ($my_file), using the one that appears first., ($file_pkg)"
+      fi
     fi
-
-    echo "ros_pkg=${ros_pkg}"
-    echo "file_pkg=${file_pkg}"
-    #check if both packages are the same
-    if [[ ${ros_pkg} != ${file_pkg} ]]
-    then
-      #modify package.xml to add dependency
-      add_pkg_to_packagexml ${ros_pkg} ${file_pkg} 
-    fi
-
   else
     found_it=false
   fi
 }
 
-# add_pkg_to_packagexml
-# Adds $2 as dependency to $1 package.xml file.
-# - $1: current ros package
-# - $2: message ros package
-function add_pkg_to_packagexml
-{
-  if [[ -z $1 ]] || [[ -z $2 ]]
-  then
-    kill_exit "add_pkg_to_packagexml: missing input parameters"
-  fi
-
-  #get path where message is stored
-  local ros_pkg=$1
-  local file_pkg=$2
-
-  #go to package folder
-  roscd "${ros_pkg}"
-
-  #look for package in package.xml and add dependency if necessary
-  local pub="<build_depend>${file_pkg}<build_depend/>"
-  local pub1=$(grep "${pub}" "package.xml")
-  if [[ -z "${pub1}" ]]
-  then
-    echo "Adding package \"${file_pkg}\" as dependency..."
-    local comment="\"iri_base_"
-    pub="<build_depend>${file_pkg}<build_depend/>"
-    sed -i -e "/${comment}/a\\  ${pub}" "package.xml"
-  else
-    echo "Package \"${file_pkg}\" already added as dependency in package.xml, skipping"
-  fi
-  
-  add_pkg_dependency_to_readme $ros_pkg $file_pkg
-}
-
-# add_pkg_to_readme
+# add_pkg_dependency_to_readme
 # Adds $2 as dependency to README.md file.
 # - $1: current ros package
 # - $2: message ros package
@@ -296,7 +256,7 @@ function add_pkg_dependency_to_readme
 {
   if [[ -z $1 ]] || [[ -z $2 ]]
   then
-    kill_exit "add_pkg_to_readme: missing input parameters"
+    kill_exit "ERROR: add_pkg_dependency_to_readme: missing input parameters"
   fi
 
   local ros_pkg=$1
@@ -306,20 +266,6 @@ function add_pkg_dependency_to_readme
   roscd "${ros_pkg}"
   
   file=README.md
-
-#   #look for package in package.xml and add dependency if necessary
-#   local pub="- ${file_pkg}"
-#   local pub1=$(grep "${pub}" ${file})
-#   if [[ -z "${pub1}" ]]
-#   then
-#     echo "Adding package \"${file_pkg}\" as dependency..."
-#     local comment="##Dependencies"
-#     pub="- ${file_pkg}"
-#     sed -i -e "/${comment}/a\\  ${pub}" ${file}
-#   else
-#     echo "Package \"${file_pkg}\" already added as dependency in README, skipping"
-#   fi
-  
   line="\- ${file_pkg}"
   comment="## Dependencies"
   add_line_to_file "${line}" "${comment}" "${file}"
@@ -337,7 +283,7 @@ function fill_readme_ros_interface # ${type} ${pub_subs} ${ros_pkg} ${topic_name
 {
   if [[ -z $1 ]] || [[ -z $2 ]] || [[ -z $3 ]] || [[ -z $4 ]] || [[ -z $5 ]] || [[ -z $6 ]]
   then
-    kill_exit "fill_readme_ros_interface missing input parameters"
+    kill_exit "ERROR: fill_readme_ros_interface missing input parameters"
   fi
   
   #echo "fill_readme_ros_interface $1 $2 $3 $4 $5 $6"
@@ -399,7 +345,7 @@ function find_comment_in_file
 {
   if [[ -z $1 ]] || [[ -z $2 ]]
   then
-    kill_exit "find_comment_in_file: missing input parameters"
+    kill_exit "ERROR: find_comment_in_file: missing input parameters"
   fi
 
   local comment_to_find=$1
@@ -424,7 +370,7 @@ function text_found_in_file
 {
   if [[ -z $1 ]] || [[ -z $2 ]]
   then
-    kill_exit "text_found_in_file missing input parameters"
+    kill_exit "ERROR: text_found_in_file missing input parameters"
   fi
 
   local text_to_find=$1
@@ -452,7 +398,7 @@ function add_line_to_file
   #echo "add_line_to_file $1 $2 $3"
   if [[ -z $1 ]] || [[ -z $2 ]] || [[ -z $3 ]]
   then
-    kill_exit "add_line_to_file: missing input parameters"
+    kill_exit "ERROR: add_line_to_file: missing input parameters"
   fi
 
   local line_to_add=$1
@@ -463,11 +409,11 @@ function add_line_to_file
   find_comment_in_file "${comment_to_find}" "${file}"
   if [[ "${comment_found}" = "false" ]]
   then
-    kill_exit "File ${file} needs to be restored, missing comment: \"${comment_to_find}\""
+    kill_exit "ERROR: File ${file} needs to be restored, missing comment: \"${comment_to_find}\""
   fi
 
   #check if line already exists in file
-  local line_found=$(grep "${line_to_add}" "${file}")
+  local line_found=$(grep "^${line_to_add}" "${file}")
 
   if [[ -z "${line_found}" ]]
   then
@@ -488,7 +434,7 @@ function get_h_cpp_files
 {
   if [[ -z $1 ]]
   then
-    kill_exit "get_h_cpp_files: missing input parameters"
+    kill_exit "ERROR: get_h_cpp_files: missing input parameters"
   fi
 
   local ros_pkg=$1
@@ -500,7 +446,7 @@ function get_h_cpp_files
   then
     if [[ -z `ls -l "include/${ros_pkg}" | grep "^-" | grep "\.h$"` ]]
     then
-      kill_exit "no include files found!"
+      kill_exit "ERROR: no include files found!"
     else
       node_h=$( ls "include/${ros_pkg}" | grep "\node.h$" )
       node_h="include/${ros_pkg}/${node_h}"
@@ -518,7 +464,7 @@ function is_driver_or_alg_node
 {
   if [[ -z $1 ]]
   then
-    kill_exit "is_driver_or_alg_node: missing input parameters"
+    kill_exit "ERROR: is_driver_or_alg_node: missing input parameters"
   fi
 
   local ros_pkg=$1
@@ -542,7 +488,7 @@ function is_driver_or_alg_node
 
 function change_license_to_LGPL
 {
-      find . -name package.xml -exec sed -i -e 's:<license>.*</license>:<license>LGPL</license>:g' {} \;
+  find . -name package.xml -exec sed -i -e 's:<license>.*</license>:<license>LGPL</license>:g' {} \;
 }
 
 function goto_catkin_workspace
@@ -661,6 +607,7 @@ function add_build_run_dependencies
   local type=
   local dep=
   local dep2="build_depend"
+  local dep3="exec_depend"
   
   if [[ "${driver_alg}" = "driver" ]]
   then
@@ -677,15 +624,16 @@ function add_build_run_dependencies
     if [[ "${text_found}" = "true"  ]]
     then
       dep="depend"
-      line="<${dep}>${new_pkg}</${dep}>"
+      line="^\s*<${dep}>${new_pkg}</${dep}>"
       find_comment_in_file "${line}" "package.xml"
       if [[ "${comment_found}" = "false" ]]
       then
+        echo "Adding $new_pkg $dep to package.xml"
         line="\ \ <${dep}>${new_pkg}<\/${dep}>"
-        comment="<${dep2}>iri_base_${type}<\/${dep2}>"
+        comment="<${dep3}>iri_base_${type}<\/${dep3}>"
         add_line_to_file "${line}" "${comment}" "package.xml"
       else
-        echo "$dep dependency already included."
+        echo "$dep dependency for $new_pkg already included in package.xml"
       fi
     else #format 1: <package>
       dep="build_depend"
@@ -697,7 +645,7 @@ function add_build_run_dependencies
         comment="<${dep2}>iri_base_${type}<\/${dep2}>"
         add_line_to_file "${line}" "${comment}" "package.xml"
       else
-        echo "$dep dependency already included."
+        echo "$dep dependency for $new_pkg already included in package.xml"
       fi
       
       dep="run_depend"
@@ -709,9 +657,11 @@ function add_build_run_dependencies
         comment="<${dep2}>iri_base_${type}<\/${dep2}>"
         add_line_to_file "${line}" "${comment}" "package.xml"
       else
-        echo "$dep dependency already included."
+        echo "$dep dependency for $new_pkg already included in package.xml"
       fi
     fi
+    
+    add_pkg_dependency_to_readme $ros_pkg $new_pkg
 
   fi
 }
@@ -769,4 +719,4 @@ function add_cmake_dependencies
 # -w filename   Check if file is writable
 # -x filename   Check if file is executable
 # -n variable   Check if variable is not null (contains one or more characaters)
-# -z variable   Check if variable is null (empty)
+# -z variable   Check if variable is null (empty)
\ No newline at end of file