From 8840a58910cbe34b9a5deaa4967e92f9647c7028 Mon Sep 17 00:00:00 2001
From: Niklas Yann Wettengel <niyawe@uni-koblenz.de>
Date: Thu, 3 Nov 2016 22:26:16 +0100
Subject: [PATCH] fixes

---
 homer_map_manager/CMakeLists.txt              | 34 ++++++++---
 .../homer_map_manager/map_manager_node.h      |  1 -
 homer_map_manager/package.xml                 | 15 ++++-
 homer_map_manager/src/map_manager_node.cpp    | 10 ++--
 homer_mapnav_msgs/package.xml                 |  2 +-
 homer_mapping/CMakeLists.txt                  | 33 ++++++-----
 .../homer_mapping/ParticleFilter/SlamFilter.h |  2 -
 .../include/homer_mapping/slam_node.h         |  2 -
 homer_mapping/package.xml                     |  8 +--
 .../src/OccupancyMap/OccupancyMap.cpp         | 17 +++---
 .../src/ParticleFilter/SlamFilter.cpp         | 36 ++++++------
 homer_mapping/src/slam_node.cpp               | 10 ++--
 homer_nav_libs/CMakeLists.txt                 | 10 ++++
 homer_nav_libs/package.xml                    |  6 +-
 .../src/SpeedControl/SpeedControl.cpp         | 12 ++--
 homer_navigation/CMakeLists.txt               | 21 ++++++-
 .../homer_navigation/homer_navigation_node.h  |  3 -
 homer_navigation/package.xml                  | 20 +++----
 .../src/homer_navigation_node.cpp             | 56 +++++++++----------
 19 files changed, 174 insertions(+), 124 deletions(-)

diff --git a/homer_map_manager/CMakeLists.txt b/homer_map_manager/CMakeLists.txt
index 741238e2..c659d0ab 100644
--- a/homer_map_manager/CMakeLists.txt
+++ b/homer_map_manager/CMakeLists.txt
@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(homer_map_manager)
 
-find_package(catkin REQUIRED COMPONENTS roscpp roslib tf homer_mapnav_msgs homer_nav_libs cmake_modules homer_tools)
+find_package(catkin REQUIRED COMPONENTS roscpp roslib tf homer_mapnav_msgs homer_nav_libs cmake_modules std_srvs)
 
 find_package( Eigen3 REQUIRED )
 
@@ -24,23 +24,29 @@ set(Managers_SRC
 	src/Managers/RoiManager.cpp
 )
 
-add_library(Managers ${Managers_SRC})
-target_link_libraries(Managers image_io)
-add_dependencies(Managers homer_mapnav_msgs_gencpp)
+add_library(homerManagers ${Managers_SRC})
+target_link_libraries(homerManagers homerImage_io)
+add_dependencies(homerManagers ${catkin_EXPORTED_TARGETS})
 
 
-add_library(image_io
+add_library(homerImage_io
 	src/MapIO/image_loader.cpp
 	src/MapIO/map_saver.cpp
 	src/MapIO/map_loader.cpp
 )
-target_link_libraries(image_io SDL SDL_image yaml-cpp)
-add_dependencies(image_io homer_mapnav_msgs_gencpp)
+target_link_libraries(homerImage_io SDL SDL_image yaml-cpp)
+add_dependencies(homerImage_io ${catkin_EXPORTED_TARGETS})
 
 catkin_package(
 	INCLUDE_DIRS include
 	CATKIN_DEPENDS
 		roscpp
+		roslib
+		tf
+		homer_mapnav_msgs
+		homer_nav_libs
+		std_srvs
+	LIBRARIES homerImage_io homerManagers
 )
 
 add_executable(map_manager src/map_manager_node.cpp)
@@ -48,11 +54,21 @@ add_executable(map_manager src/map_manager_node.cpp)
 target_link_libraries(
 	map_manager
 		${catkin_LIBRARIES}
-		Managers
-		image_io
+		homerManagers
+		homerImage_io
 )
 
 add_dependencies(
 	map_manager
 		${catkin_EXPORTED_TARGETS}
 )
+
+install(DIRECTORY include/${PROJECT_NAME}/
+	DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+install(TARGETS homerManagers homerImage_io map_manager
+	ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+	LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+	RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
diff --git a/homer_map_manager/include/homer_map_manager/map_manager_node.h b/homer_map_manager/include/homer_map_manager/map_manager_node.h
index 19873a71..13464036 100644
--- a/homer_map_manager/include/homer_map_manager/map_manager_node.h
+++ b/homer_map_manager/include/homer_map_manager/map_manager_node.h
@@ -28,7 +28,6 @@
 
 #include <homer_map_manager/MapIO/map_saver.h>
 #include <homer_map_manager/MapIO/map_loader.h>
-#include <homer_tools/loadRosConfig.h>
 #include <homer_nav_libs/tools.h>
 
 /** @class MapManagerNode
diff --git a/homer_map_manager/package.xml b/homer_map_manager/package.xml
index 2d4deb68..c2e2b2bd 100644
--- a/homer_map_manager/package.xml
+++ b/homer_map_manager/package.xml
@@ -1,6 +1,6 @@
 <package>
   <name>homer_map_manager</name>
-  <version>1.0.0</version>
+  <version>0.1.0</version>
   <description>
      map_manager
   </description>
@@ -15,14 +15,23 @@
   <build_depend>tf</build_depend>
   <build_depend>homer_mapnav_msgs</build_depend>
   <build_depend>homer_nav_libs</build_depend>
-  <build_depend>homer_tools</build_depend>
+  <build_depend>cmake_modules</build_depend>
+  <build_depend>eigen</build_depend>
+  <build_depend>sdl</build_depend>
+  <build_depend>sdl-image</build_depend>
+  <build_depend>yaml-cpp</build_depend>
+  <build_depend>std_srvs</build_depend>
   
   <run_depend>roscpp</run_depend>
   <run_depend>roslib</run_depend>
   <run_depend>tf</run_depend>
   <run_depend>homer_mapnav_msgs</run_depend>
   <run_depend>homer_nav_libs</run_depend>
-  <run_depend>homer_tools</run_depend>
+  <run_depend>eigen</run_depend>
+  <run_depend>sdl</run_depend>
+  <run_depend>sdl-image</run_depend>
+  <run_depend>yaml-cpp</run_depend>
+  <run_depend>std_srvs</run_depend>
 
 </package>
 
diff --git a/homer_map_manager/src/map_manager_node.cpp b/homer_map_manager/src/map_manager_node.cpp
index 252e7281..59bca474 100644
--- a/homer_map_manager/src/map_manager_node.cpp
+++ b/homer_map_manager/src/map_manager_node.cpp
@@ -7,10 +7,10 @@ MapManagerNode::MapManagerNode(ros::NodeHandle *nh)
 
     int mapSize;
     float resolution;
-    loadConfigValue("/homer_mapping/size", mapSize, (int) 35);
-    loadConfigValue("/homer_mapping/resolution", resolution, (float) 0.05);
-    loadConfigValue("/map_manager/roi_updates", m_roi_polling, (bool) false);
-    loadConfigValue("/map_manager/roi_polling_time", m_roi_polling_time, (float) 0.5);
+	ros::param::param("/homer_mapping/size", mapSize, (int) 35);
+	ros::param::param("/homer_mapping/resolution", resolution, (float) 0.05);
+    ros::param::param("/map_manager/roi_updates", m_roi_polling, (bool) false);
+    ros::param::param("/map_manager/roi_polling_time", m_roi_polling_time, (float) 0.5);
     m_lastROIPoll = ros::Time::now();
     m_MapManager = new MapManager(nh);
     m_POIManager = new PoiManager(nh);
@@ -66,7 +66,7 @@ MapManagerNode::MapManagerNode(ros::NodeHandle *nh)
 
     //load map file from config if present
     std::string mapfile;
-    loadConfigValue("/map_manager/default_mapfile", mapfile);
+	ros::param::get("/map_manager/default_mapfile", mapfile);
     if(mapfile != "")
     {
 		std_msgs::String::Ptr mapfileMsg(new std_msgs::String);
diff --git a/homer_mapnav_msgs/package.xml b/homer_mapnav_msgs/package.xml
index 7c319ec9..536fe91f 100644
--- a/homer_mapnav_msgs/package.xml
+++ b/homer_mapnav_msgs/package.xml
@@ -1,6 +1,6 @@
 <package>
   <name>homer_mapnav_msgs</name>
-  <version>1.0.0</version>
+  <version>0.1.0</version>
   <description> homer_mapnav_msgs contains the messages used for mapping and navigation
   </description>
   <maintainer email="vseib@uni-koblenz.de">Viktor Seib</maintainer>
diff --git a/homer_mapping/CMakeLists.txt b/homer_mapping/CMakeLists.txt
index c76eaafb..fee377db 100644
--- a/homer_mapping/CMakeLists.txt
+++ b/homer_mapping/CMakeLists.txt
@@ -10,14 +10,12 @@ find_package(
 		homer_nav_libs
 		tf
 		roslib
-		homer_tools
 )
 find_package(OpenMP)
 if (OPENMP_FOUND)
     set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
     set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
 endif()
-find_package(Qt4 COMPONENTS QtCore QtGui REQUIRED)
 find_package(tf REQUIRED)
 
 set(CMAKE_BUILD_TYPE Release)
@@ -26,7 +24,6 @@ set(CMAKE_BUILD_TYPE Release)
 include_directories(
 	include
 	${catkin_INCLUDE_DIRS}
-	${QT_INCLUDE_DIR}
 )
 
 catkin_package(
@@ -35,20 +32,20 @@ catkin_package(
 		roscpp
 		homer_mapnav_msgs
 		homer_nav_libs
+		nav_msgs
+		tf
+		roslib
+		LIBRARIES homerOccupancyMap homerParticleFilter
 )
 
-include(${QT_USE_FILE})
-add_definitions(${QT_DEFINITIONS})
-
-add_library( OccupancyMap
+add_library(homerOccupancyMap
 	src/OccupancyMap/OccupancyMap.cpp
 	
 )
 
 target_link_libraries(
-	OccupancyMap
+	homerOccupancyMap
 		${catkin_LIBRARIES}
-		${QT_LIBRARIES}
 		${tf_LIBRARIES}
 )
 set(
@@ -60,12 +57,12 @@ set(
 )
 
 add_library(
-	ParticleFilter
+	homerParticleFilter
 		${ParticleFilter_SRC}
 )
 
 target_link_libraries(
-	ParticleFilter
+	homerParticleFilter
 		${catkin_LIBRARIES}
 )
 
@@ -74,11 +71,21 @@ add_executable(homer_mapping src/slam_node.cpp)
 target_link_libraries(
 	homer_mapping
 		${catkin_LIBRARIES}
-		ParticleFilter
-		OccupancyMap
+		homerParticleFilter
+		homerOccupancyMap
 )
 
 add_dependencies(
 	homer_mapping
 		${catkin_EXPORTED_TARGETS}
 )
+
+install(DIRECTORY include/${PROJECT_NAME}/
+	DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+install(TARGETS homerOccupancyMap homerParticleFilter homer_mapping
+	ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+	LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+	RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
diff --git a/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h b/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h
index e12e13fc..a712fb39 100644
--- a/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h
+++ b/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h
@@ -11,8 +11,6 @@
 #include <homer_nav_libs/Math/Transformation2D.h>
 #include <homer_nav_libs/Math/Math.h>
 
-#include <homer_tools/loadRosConfig.h>
-
 #include <tf/transform_broadcaster.h>
 
 #include <cmath>
diff --git a/homer_mapping/include/homer_mapping/slam_node.h b/homer_mapping/include/homer_mapping/slam_node.h
index 1505c916..5e4081a4 100644
--- a/homer_mapping/include/homer_mapping/slam_node.h
+++ b/homer_mapping/include/homer_mapping/slam_node.h
@@ -27,8 +27,6 @@
 #include <nav_msgs/OccupancyGrid.h>
 #include <tf/tf.h>
 
-#include <homer_tools/loadRosConfig.h>
-
 #include <homer_mapping/ParticleFilter/SlamFilter.h>
 #include <homer_mapping/ParticleFilter/HyperSlamFilter.h>
 #include <homer_nav_libs/Math/Box2D.h>
diff --git a/homer_mapping/package.xml b/homer_mapping/package.xml
index ddc93733..aca80902 100644
--- a/homer_mapping/package.xml
+++ b/homer_mapping/package.xml
@@ -1,6 +1,6 @@
 <package>
   <name>homer_mapping</name>
-  <version>1.0.0</version>
+  <version>0.1.0</version>
   <description>
 
      homer_mapping
@@ -18,9 +18,8 @@
   <build_depend>tf</build_depend>
   <build_depend>roslib</build_depend>
   <build_depend>homer_nav_libs</build_depend>
-
   <build_depend>homer_mapnav_msgs</build_depend>
-  <build_depend>homer_tools</build_depend>
+  <build_depend>cmake_modules</build_depend>
  
 
   <run_depend>std_msgs</run_depend>
@@ -30,7 +29,6 @@
   <run_depend>tf</run_depend>
   <run_depend>roslib</run_depend>  
   <run_depend>homer_nav_libs</run_depend>
-
   <run_depend>homer_mapnav_msgs</run_depend>
-  <run_depend>homer_tools</run_depend>
+
 </package>
diff --git a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
index d6359f24..5ab3b94a 100644
--- a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
+++ b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
@@ -14,7 +14,6 @@
 #include <tf/transform_listener.h>
 
 #include <homer_mapnav_msgs/ModifyMap.h>
-#include <homer_tools/loadRosConfig.h>
 #include <homer_nav_libs/tools.h>
 
 //uncomment this to get extended information on the tracer
@@ -91,9 +90,9 @@ OccupancyMap::~OccupancyMap()
 void OccupancyMap::initMembers()
 {
   float mapSize;
-  loadConfigValue("/homer_mapping/size", mapSize);
-  loadConfigValue("/homer_mapping/resolution", m_Resolution);
-  loadConfigValue("/homer_mapping/max_laser", m_LaserMaxRange, (float) 8.0);
+  ros::param::get("/homer_mapping/size", mapSize);
+  ros::param::get("/homer_mapping/resolution", m_Resolution);
+  ros::param::param("/homer_mapping/max_laser", m_LaserMaxRange, (float) 8.0);
 
   //add one safety pixel
   m_PixelSize = mapSize / m_Resolution + 1;
@@ -106,10 +105,10 @@ void OccupancyMap::initMembers()
   m_Origin.orientation.y = 0.0;
   m_Origin.orientation.z = 0.0;
 
-  loadConfigValue("/homer_mapping/backside_checking", m_BacksideChecking);
-  loadConfigValue("/homer_mapping/obstacle_borders", m_ObstacleBorders);
-  loadConfigValue("/homer_mapping/measure_sampling_step", m_MeasureSamplingStep);
-  loadConfigValue("/homer_mapping/laser_scanner/free_reading_distance", m_FreeReadingDistance);
+  ros::param::get("/homer_mapping/backside_checking", m_BacksideChecking);
+  ros::param::get("/homer_mapping/obstacle_borders", m_ObstacleBorders);
+  ros::param::get("/homer_mapping/measure_sampling_step", m_MeasureSamplingStep);
+  ros::param::get("/homer_mapping/laser_scanner/free_reading_distance", m_FreeReadingDistance);
 
   m_OccupancyProbability = new float[m_ByteSize];
   m_MeasurementCount = new unsigned short[m_ByteSize];
@@ -158,7 +157,7 @@ OccupancyMap& OccupancyMap::operator= ( const OccupancyMap & occupancyMap )
   m_PixelSize = occupancyMap.m_PixelSize;
   m_ByteSize = occupancyMap.m_ByteSize;
 
-  loadConfigValue("/homer_mapping/backside_checking", m_BacksideChecking);
+  ros::param::get("/homer_mapping/backside_checking", m_BacksideChecking);
 
   // re-allocate all arrays
   m_OccupancyProbability = new float[m_ByteSize];
diff --git a/homer_mapping/src/ParticleFilter/SlamFilter.cpp b/homer_mapping/src/ParticleFilter/SlamFilter.cpp
index 33c28049..628ae187 100644
--- a/homer_mapping/src/ParticleFilter/SlamFilter.cpp
+++ b/homer_mapping/src/ParticleFilter/SlamFilter.cpp
@@ -20,23 +20,23 @@ SlamFilter::SlamFilter ( int particleNum ) : ParticleFilter<SlamParticle> ( part
   }
 
   float rotationErrorRotating = 0.0;
-  loadConfigValue("/particlefilter/error_values/rotation_error_rotating", rotationErrorRotating);
+  ros::param::get("/particlefilter/error_values/rotation_error_rotating", rotationErrorRotating);
   float rotationErrorTranslating = 0.0;
-  loadConfigValue("/particlefilter/error_values/rotation_error_translating", rotationErrorTranslating);
+  ros::param::get("/particlefilter/error_values/rotation_error_translating", rotationErrorTranslating);
   float translationErrorTranslating = 0.0;
-  loadConfigValue("/particlefilter/error_values/translation_error_translating", translationErrorTranslating);
+  ros::param::get("/particlefilter/error_values/translation_error_translating", translationErrorTranslating);
   float translationErrorRotating = 0.0;
-  loadConfigValue("/particlefilter/error_values/translation_error_translating", translationErrorRotating);
+  ros::param::get("/particlefilter/error_values/translation_error_translating", translationErrorRotating);
   float moveJitterWhileTurning = 0.0;
-  loadConfigValue("/particlefilter/error_values/move_jitter_while_turning", moveJitterWhileTurning);
-  loadConfigValue("/particlefilter/max_rotation_per_second", m_MaxRotationPerSecond);
+  ros::param::get("/particlefilter/error_values/move_jitter_while_turning", moveJitterWhileTurning);
+  ros::param::get("/particlefilter/max_rotation_per_second", m_MaxRotationPerSecond);
 
   int updateMinMoveAngleDegrees;
-  loadConfigValue("/particlefilter/update_min_move_angle", updateMinMoveAngleDegrees);
+  ros::param::get("/particlefilter/update_min_move_angle", updateMinMoveAngleDegrees);
   m_UpdateMinMoveAngle = Math::deg2Rad(updateMinMoveAngleDegrees);
-  loadConfigValue("/particlefilter/update_min_move_dist", m_UpdateMinMoveDistance);
+  ros::param::get("/particlefilter/update_min_move_dist", m_UpdateMinMoveDistance);
   double maxUpdateInterval;
-  loadConfigValue("/particlefilter/max_update_interval", maxUpdateInterval);
+  ros::param::get("/particlefilter/max_update_interval", maxUpdateInterval);
   m_MaxUpdateInterval = ros::Duration(maxUpdateInterval);
 
   setRotationErrorRotating ( rotationErrorRotating );
@@ -77,23 +77,23 @@ SlamFilter::SlamFilter ( SlamFilter& slamFilter ) : ParticleFilter<SlamParticle>
   }
 
   float rotationErrorRotating = 0.0;
-  loadConfigValue("/particlefilter/error_values/rotation_error_rotating", rotationErrorRotating);
+  ros::param::get("/particlefilter/error_values/rotation_error_rotating", rotationErrorRotating);
   float rotationErrorTranslating = 0.0;
-  loadConfigValue("/particlefilter/error_values/rotation_error_translating", rotationErrorTranslating);
+  ros::param::get("/particlefilter/error_values/rotation_error_translating", rotationErrorTranslating);
   float translationErrorTranslating = 0.0;
-  loadConfigValue("/particlefilter/error_values/translation_error_translating", translationErrorTranslating);
+  ros::param::get("/particlefilter/error_values/translation_error_translating", translationErrorTranslating);
   float translationErrorRotating = 0.0;
-  loadConfigValue("/particlefilter/error_values/translation_error_translating", translationErrorRotating);
+  ros::param::get("/particlefilter/error_values/translation_error_translating", translationErrorRotating);
   float moveJitterWhileTurning = 0.0;
-  loadConfigValue("/particlefilter/error_values/move_jitter_while_turning", moveJitterWhileTurning);
-  loadConfigValue("/particlefilter/max_rotation_per_second", m_MaxRotationPerSecond);
+  ros::param::get("/particlefilter/error_values/move_jitter_while_turning", moveJitterWhileTurning);
+  ros::param::get("/particlefilter/max_rotation_per_second", m_MaxRotationPerSecond);
 
   int updateMinMoveAngleDegrees;
-  loadConfigValue("/particlefilter/update_min_move_angle", updateMinMoveAngleDegrees);
+  ros::param::get("/particlefilter/update_min_move_angle", updateMinMoveAngleDegrees);
   m_UpdateMinMoveAngle = Math::deg2Rad(updateMinMoveAngleDegrees);
-  loadConfigValue("/particlefilter/update_min_move_dist", m_UpdateMinMoveDistance);
+  ros::param::get("/particlefilter/update_min_move_dist", m_UpdateMinMoveDistance);
   double maxUpdateInterval;
-  loadConfigValue("/particlefilter/max_update_interval", maxUpdateInterval);
+  ros::param::get("/particlefilter/max_update_interval", maxUpdateInterval);
   m_MaxUpdateInterval = ros::Duration(maxUpdateInterval);
 
   setRotationErrorRotating ( rotationErrorRotating );
diff --git a/homer_mapping/src/slam_node.cpp b/homer_mapping/src/slam_node.cpp
index 9b906130..c80692b2 100644
--- a/homer_mapping/src/slam_node.cpp
+++ b/homer_mapping/src/slam_node.cpp
@@ -44,17 +44,17 @@ SlamNode::SlamNode(ros::NodeHandle* nh)
 void SlamNode::init()
 {
     double waitTime;
-    loadConfigValue("/particlefilter/wait_time", waitTime);
+	ros::param::get("/particlefilter/wait_time", waitTime);
     m_WaitDuration = ros::Duration(waitTime);
-    loadConfigValue("/selflocalization/scatter_var_xy", m_ScatterVarXY);
-    loadConfigValue("/selflocalization/scatter_var_theta", m_ScatterVarTheta);
+	ros::param::get("/selflocalization/scatter_var_xy", m_ScatterVarXY);
+	ros::param::get("/selflocalization/scatter_var_theta", m_ScatterVarTheta);
 
     m_DoMapping = true;
 
     int particleNum;
-    loadConfigValue("/particlefilter/particle_num", particleNum);
+	ros::param::get("/particlefilter/particle_num", particleNum);
     int particleFilterNum;
-    loadConfigValue("/particlefilter/hyper_slamfilter/particlefilter_num", particleFilterNum);
+	ros::param::get("/particlefilter/hyper_slamfilter/particlefilter_num", particleFilterNum);
     m_HyperSlamFilter = new HyperSlamFilter ( particleFilterNum, particleNum );
 
     m_ReferenceOdometryTime = ros::Time(0);
diff --git a/homer_nav_libs/CMakeLists.txt b/homer_nav_libs/CMakeLists.txt
index bc5efcbd..c3682171 100644
--- a/homer_nav_libs/CMakeLists.txt
+++ b/homer_nav_libs/CMakeLists.txt
@@ -32,3 +32,13 @@ add_library(homerMappingMath
 	src/Math/Math.cpp
 	src/Math/Point2D.cpp
 )
+
+install(DIRECTORY include/${PROJECT_NAME}/
+	DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+install(TARGETS homerExplorer homerMappingMath
+	ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+	LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+	RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
diff --git a/homer_nav_libs/package.xml b/homer_nav_libs/package.xml
index 117158f9..ab43684c 100644
--- a/homer_nav_libs/package.xml
+++ b/homer_nav_libs/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>homer_nav_libs</name>
-  <version>1.0.0</version>
+  <version>0.1.0</version>
   <description>The nav_libs package</description>
 
   <maintainer email="vseib@uni-koblenz.de">Viktor Seib</maintainer>
@@ -12,8 +12,12 @@
   <build_depend>geometry_msgs</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>tf</build_depend>
+  <build_depend>cmake_modules</build_depend>
+  <build_depend>eigen</build_depend>
+
   <run_depend>roscpp</run_depend>
   <run_depend>geometry_msgs</run_depend>
   <run_depend>tf</run_depend>
+  <run_depend>eigen</run_depend>
 
 </package>
diff --git a/homer_nav_libs/src/SpeedControl/SpeedControl.cpp b/homer_nav_libs/src/SpeedControl/SpeedControl.cpp
index ddb72a94..c939197e 100644
--- a/homer_nav_libs/src/SpeedControl/SpeedControl.cpp
+++ b/homer_nav_libs/src/SpeedControl/SpeedControl.cpp
@@ -38,10 +38,10 @@ namespace {
   inline Eigen::AlignedBox2f loadRect(const string& path)
   {
     pair<float, float> pX, pY;
-    loadConfigValue(path + "/x_min", pX.first);
-    loadConfigValue(path + "/x_max", pX.second);
-    loadConfigValue(path + "/y_min", pY.first);
-    loadConfigValue(path + "/y_max", pY.second);
+	ros::param::get(path + "/x_min", pX.first);
+	ros::param::get(path + "/x_max", pX.second);
+	ros::param::get(path + "/y_min", pY.first);
+	ros::param::get(path + "/y_max", pY.second);
 
     Eigen::Vector2f first(pX.first, pY.first), second(pX.second, pY.second);
     return Eigen::AlignedBox2f(first, second);
@@ -52,10 +52,10 @@ void SpeedControl::loadDimensions()
 {
   InnerDangerZone = loadRect("/homer_navigation/speed_control/inner_danger_zone");
   InnerDangerZoneFactor;
-  loadConfigValue("/homer_navigation/speed_control/inner_danger_zone/speed_factor", InnerDangerZoneFactor);
+  ros::param::get("/homer_navigation/speed_control/inner_danger_zone/speed_factor", InnerDangerZoneFactor);
   OuterDangerZone = loadRect("/homer_navigation/speed_control/inner_danger_zone");
   OuterDangerZoneFactor;
-  loadConfigValue("/homer_navigation/speed_control/outer_danger_zone/speed_factor", OuterDangerZoneFactor);
+  ros::param::get("/homer_navigation/speed_control/outer_danger_zone/speed_factor", OuterDangerZoneFactor);
   if(!OuterDangerZone.contains(InnerDangerZone))
     ROS_WARN_STREAM("InnerDangerZone is not contained in OuterDangerZone");
 }
diff --git a/homer_navigation/CMakeLists.txt b/homer_navigation/CMakeLists.txt
index 98245afb..94d03307 100644
--- a/homer_navigation/CMakeLists.txt
+++ b/homer_navigation/CMakeLists.txt
@@ -5,14 +5,12 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   roslib
   homer_robbie_architecture
-  homer_robot_platform
   nav_msgs
   sensor_msgs
   homer_mapnav_msgs
   homer_nav_libs
   tf
   cmake_modules
-  homer_tools
 )
 
 find_package(Eigen3 REQUIRED)
@@ -21,6 +19,15 @@ set(CMAKE_BUILD_TYPE Release)
 
 catkin_package(
   INCLUDE_DIRS include
+  CATKIN_DEPENDS
+  	roscpp
+	roslib
+	homer_robbie_architecture
+	nav_msgs
+	sensor_msgs
+	homer_mapnav_msgs
+	homer_nav_libs
+	tf
 )
 
 include_directories(
@@ -36,3 +43,13 @@ target_link_libraries(homer_navigation
    ${catkin_LIBRARIES}
    ${EIGEN3_LIBRARIES}
  )
+
+install(DIRECTORY include/${PROJECT_NAME}/
+	DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+install(TARGETS homer_navigation
+	ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+	LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+	RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
diff --git a/homer_navigation/include/homer_navigation/homer_navigation_node.h b/homer_navigation/include/homer_navigation/homer_navigation_node.h
index cc3b4391..a273994b 100644
--- a/homer_navigation/include/homer_navigation/homer_navigation_node.h
+++ b/homer_navigation/include/homer_navigation/homer_navigation_node.h
@@ -30,9 +30,6 @@
 #include <homer_ptu_msgs/CenterWorldPoint.h>
 #include <homer_ptu_msgs/SetPanTilt.h>
 
-#include <homer_tools/loadRosConfig.h>
-
-//nav_libs
 #include <homer_nav_libs/tools.h>
 #include <homer_nav_libs/Explorer/Explorer.h>
 
diff --git a/homer_navigation/package.xml b/homer_navigation/package.xml
index ef223ca0..3515cfb7 100644
--- a/homer_navigation/package.xml
+++ b/homer_navigation/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>homer_navigation</name>
-  <version>1.0.0</version>
+  <version>0.1.0</version>
   <description>The homer_navigation package</description>
 
   <maintainer email="vseib@uni-koblenz.de">Viktor Seib</maintainer>
@@ -13,26 +13,24 @@
   
   <build_depend>roscpp</build_depend>
   <build_depend>roslib</build_depend>
-  <build_depend>robbie_architecture</build_depend>
-  <build_depend>robot_platform</build_depend>
-  <build_depend>nav_libs</build_depend>
+  <build_depend>homer_robbie_architecture</build_depend>
+  <build_depend>homer_nav_libs</build_depend>
   <build_depend>nav_msgs</build_depend>
   <build_depend>sensor_msgs</build_depend>
-  <build_depend>mapnav_msgs</build_depend>
+  <build_depend>homer_mapnav_msgs</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>tf</build_depend>
-  <build_depend>homer_tools</build_depend>
+  <build_depend>cmake_modules</build_depend>
+  <build_depend>eigen</build_depend>
   
   <run_depend>roscpp</run_depend>
   <run_depend>roslib</run_depend>
-  <run_depend>robbie_architecture</run_depend>
-  <run_depend>robot_platform</run_depend>
-  <run_depend>nav_libs</run_depend>
+  <run_depend>homer_robbie_architecture</run_depend>
+  <run_depend>homer_nav_libs</run_depend>
   <run_depend>nav_msgs</run_depend>
   <run_depend>sensor_msgs</run_depend>
-  <run_depend>mapnav_msgs</run_depend>
+  <run_depend>homer_mapnav_msgs</run_depend>
   <run_depend>std_msgs</run_depend>
   <run_depend>tf</run_depend>
-  <run_depend>homer_tools</run_depend>
 
 </package>
diff --git a/homer_navigation/src/homer_navigation_node.cpp b/homer_navigation/src/homer_navigation_node.cpp
index 5e58c990..9bad6a76 100644
--- a/homer_navigation/src/homer_navigation_node.cpp
+++ b/homer_navigation/src/homer_navigation_node.cpp
@@ -41,52 +41,52 @@ HomerNavigationNode::HomerNavigationNode()
 void HomerNavigationNode::loadParameters()
 {
 	//Explorer constructor 
-	loadConfigValue("/homer_mapping/resolution", 						m_resolution, (double) 0.05);
-	loadConfigValue("/homer_navigation/allowed_obstacle_distance/min", 	m_AllowedObstacleDistance.first, (float) 0.3);
-	loadConfigValue("/homer_navigation/allowed_obstacle_distance/max", 	m_AllowedObstacleDistance.second, (float) 5.0);
-	loadConfigValue("/homer_navigation/safe_obstacle_distance/min", 	m_SafeObstacleDistance.first, (float) 0.7);
-	loadConfigValue("/homer_navigation/safe_obstacle_distance/max", 	m_SafeObstacleDistance.second, (float) 1.5);
-	loadConfigValue("/homer_navigation/frontier_safeness_factor", 		m_FrontierSafenessFactor, (float) 1.4);
-	loadConfigValue("/homer_navigation/safe_path_weight", 				m_SafePathWeight, (double) 1.2);
-	loadConfigValue("/homer_navigation/waypoint_sampling_threshold", 	m_waypoint_sampling_threshold, (float) 1.5);    
+	ros::param::param("/homer_mapping/resolution", 						m_resolution, (double) 0.05);
+	ros::param::param("/homer_navigation/allowed_obstacle_distance/min", 	m_AllowedObstacleDistance.first, (float) 0.3);
+	ros::param::param("/homer_navigation/allowed_obstacle_distance/max", 	m_AllowedObstacleDistance.second, (float) 5.0);
+	ros::param::param("/homer_navigation/safe_obstacle_distance/min", 	m_SafeObstacleDistance.first, (float) 0.7);
+	ros::param::param("/homer_navigation/safe_obstacle_distance/max", 	m_SafeObstacleDistance.second, (float) 1.5);
+	ros::param::param("/homer_navigation/frontier_safeness_factor", 		m_FrontierSafenessFactor, (float) 1.4);
+	ros::param::param("/homer_navigation/safe_path_weight", 				m_SafePathWeight, (double) 1.2);
+	ros::param::param("/homer_navigation/waypoint_sampling_threshold", 	m_waypoint_sampling_threshold, (float) 1.5);    
 	m_AllowedObstacleDistance.first 	/= m_resolution;
 	m_AllowedObstacleDistance.second	/= m_resolution;
 	m_SafeObstacleDistance.first 		/= m_resolution;
 	m_SafeObstacleDistance.second 		/= m_resolution;
 
 	//check path 
-	loadConfigValue("/homer_navigation/check_path", 					m_check_path, (bool) true);
-	loadConfigValue("/homer_navigation/check_path_max_distance", 		m_check_path_max_distance, (float) 2.0);
+	ros::param::param("/homer_navigation/check_path", 					m_check_path, (bool) true);
+	ros::param::param("/homer_navigation/check_path_max_distance", 		m_check_path_max_distance, (float) 2.0);
 
 	//collision 
-	loadConfigValue("/homer_navigation/collision_distance", 			m_collision_distance, (float) 0.3);
-	loadConfigValue("/homer_navigation/collision_distance_near_target", m_collision_distance_near_target, (float) 0.2);
-	loadConfigValue("/homer_navigation/backward_collision_distance", 	m_backward_collision_distance, (float) 0.5);
+	ros::param::param("/homer_navigation/collision_distance", 			m_collision_distance, (float) 0.3);
+	ros::param::param("/homer_navigation/collision_distance_near_target", m_collision_distance_near_target, (float) 0.2);
+	ros::param::param("/homer_navigation/backward_collision_distance", 	m_backward_collision_distance, (float) 0.5);
 
 	//cmd_vel config values
-	loadConfigValue("/homer_navigation/min_turn_angle", 				m_min_turn_angle, (float) 0.15);
-	loadConfigValue("/homer_navigation/max_turn_speed", 				m_max_turn_speed, (float) 0.6);
-	loadConfigValue("/homer_navigation/min_turn_speed",					m_min_turn_speed, (float) 0.3);
-	loadConfigValue("/homer_navigation/max_move_speed", 				m_max_move_speed, (float) 0.4);
-	loadConfigValue("/homer_navigation/max_drive_angle",				m_max_drive_angle, (float) 0.6);
+	ros::param::param("/homer_navigation/min_turn_angle", 				m_min_turn_angle, (float) 0.15);
+	ros::param::param("/homer_navigation/max_turn_speed", 				m_max_turn_speed, (float) 0.6);
+	ros::param::param("/homer_navigation/min_turn_speed",					m_min_turn_speed, (float) 0.3);
+	ros::param::param("/homer_navigation/max_move_speed", 				m_max_move_speed, (float) 0.4);
+	ros::param::param("/homer_navigation/max_drive_angle",				m_max_drive_angle, (float) 0.6);
 
 	//caution factors
-	loadConfigValue("/homer_navigation/map_speed_factor", 				m_map_speed_factor, (float) 1.0);	
-	loadConfigValue("/homer_navigation/waypoint_speed_factor", 			m_waypoint_speed_factor, (float) 1.0);	
-	loadConfigValue("/homer_navigation/obstacle_speed_factor", 			m_obstacle_speed_factor, (float) 1.0);	
-	loadConfigValue("/homer_navigation/target_distance_speed_factor", 	m_target_distance_speed_factor, (float) 0.4);	
+	ros::param::param("/homer_navigation/map_speed_factor", 				m_map_speed_factor, (float) 1.0);	
+	ros::param::param("/homer_navigation/waypoint_speed_factor", 			m_waypoint_speed_factor, (float) 1.0);	
+	ros::param::param("/homer_navigation/obstacle_speed_factor", 			m_obstacle_speed_factor, (float) 1.0);	
+	ros::param::param("/homer_navigation/target_distance_speed_factor", 	m_target_distance_speed_factor, (float) 0.4);	
 
 	//robot dimensions
-	loadConfigValue("/homer_navigation/min_x", 							m_min_x, (float) 0.3);
-	loadConfigValue("/homer_navigation/min_y",							m_min_y, (float) 0.27);
+	ros::param::param("/homer_navigation/min_x", 							m_min_x, (float) 0.3);
+	ros::param::param("/homer_navigation/min_y",							m_min_y, (float) 0.27);
 
 	//error durations
-	loadConfigValue("/homer_navigation/callback_error_duration", 		m_callback_error_duration, (float) 0.3);
+	ros::param::param("/homer_navigation/callback_error_duration", 		m_callback_error_duration, (float) 0.3);
 
-	loadConfigValue("/homer_navigation/use_ptu",						m_use_ptu, (bool) false);
+	ros::param::param("/homer_navigation/use_ptu",						m_use_ptu, (bool) false);
 
-	loadConfigValue("/homer_navigation/unknown_threshold", 				m_unknown_threshold, (int) 50);
-	loadConfigValue("/homer_navigation/waypoint_radius_factor",			m_waypoint_radius_factor, (float) 0.25);
+	ros::param::param("/homer_navigation/unknown_threshold", 				m_unknown_threshold, (int) 50);
+	ros::param::param("/homer_navigation/waypoint_radius_factor",			m_waypoint_radius_factor, (float) 0.25);
 }
 
 void HomerNavigationNode::init()
-- 
GitLab