diff --git a/.gitignore b/.gitignore
index c8aa9bb..cfac8dd 100644
--- a/.gitignore
+++ b/.gitignore
@@ -420,3 +420,4 @@ FodyWeavers.xsd
 
 build
 install
+devel
diff --git a/.gitmodules b/.gitmodules
index a81ef79..dbfc26f 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -19,9 +19,6 @@
 [submodule "src/Libraries/data_convert"]
 	path = src/Libraries/data_convert
 	url = https://git.pnkr.asia/DuongTD/data_convert.git
-[submodule "src/Libraries/robot_cpp"]
-	path = src/Libraries/robot_cpp
-	url = http://git.pnkx/HiepLM/robot_cpp.git
 [submodule "src/Algorithms/Packages/global_planners/custom_planner"]
 	path = src/Algorithms/Packages/global_planners/custom_planner
 	url = https://git.pnkr.asia/DuongTD/custom_planner.git
diff --git a/src/Libraries/costmap_2d b/src/Libraries/costmap_2d
index ae469e3..b6733ae 160000
--- a/src/Libraries/costmap_2d
+++ b/src/Libraries/costmap_2d
@@ -1 +1 @@
-Subproject commit ae469e3271bdcdb4f20629dd7de57553c1e7e074
+Subproject commit b6733ae04cc2ec79409faf05bbae5f70a3c7fcd2
diff --git a/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/polygons.h b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/polygons.h
index 597178f..c31b683 100755
--- a/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/polygons.h
+++ b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/polygons.h
@@ -99,7 +99,7 @@ robot_nav_2d_msgs::Polygon2D polygonFromString(const std::string& polygon_string
  *                       3 or more elements, and the sub-arrays should all have exactly 2 elements
  *                       (x and y coordinates).
  */
-robot_nav_2d_msgs::Polygon2D polygonFromXMLRPC(robot::XmlRpc::XmlRpcValue& polygon_xmlrpc);
+robot_nav_2d_msgs::Polygon2D polygonFromXMLRPC(robot_xmlrpcpp::XmlRpcValue& polygon_xmlrpc);
 
 /**
  * @brief Create XMLRPC Value for writing the polygon to the parameter server
@@ -107,7 +107,7 @@ robot_nav_2d_msgs::Polygon2D polygonFromXMLRPC(robot::XmlRpc::XmlRpcValue& polyg
  * @param array_of_arrays If true, write an array of arrays. Otherwise, write two parallel arrays
  * @return XmlRpcValue
  */
-robot::XmlRpc::XmlRpcValue polygonToXMLRPC(const robot_nav_2d_msgs::Polygon2D& polygon, bool array_of_arrays = true);
+robot_xmlrpcpp::XmlRpcValue polygonToXMLRPC(const robot_nav_2d_msgs::Polygon2D& polygon, bool array_of_arrays = true);
 
 // /**
 //  * @brief Save a polygon to a parameter
diff --git a/src/Libraries/robot_nav_2d_utils/src/polygons.cpp b/src/Libraries/robot_nav_2d_utils/src/polygons.cpp
index eb4d98b..b60e713 100755
--- a/src/Libraries/robot_nav_2d_utils/src/polygons.cpp
+++ b/src/Libraries/robot_nav_2d_utils/src/polygons.cpp
@@ -159,13 +159,13 @@ Polygon2D polygonFromString(const std::string& polygon_string)
 /**
  * @brief Helper function. Convert value to double
  */
-double getNumberFromXMLRPC(robot::XmlRpc::XmlRpcValue& value)
+double getNumberFromXMLRPC(robot_xmlrpcpp::XmlRpcValue& value)
 {
-  if (value.getType() == robot::XmlRpc::XmlRpcValue::TypeInt)
+  if (value.getType() == robot_xmlrpcpp::XmlRpcValue::TypeInt)
   {
     return static_cast<double>(static_cast<int>(value));
   }
-  else if (value.getType() == robot::XmlRpc::XmlRpcValue::TypeDouble)
+  else if (value.getType() == robot_xmlrpcpp::XmlRpcValue::TypeDouble)
   {
     return static_cast<double>(value);
   }
@@ -178,9 +178,9 @@ double getNumberFromXMLRPC(robot::XmlRpc::XmlRpcValue& value)
 /**
  * @brief Helper function. Convert value to double array
  */
-std::vector<double> getNumberVectorFromXMLRPC(robot::XmlRpc::XmlRpcValue& value)
+std::vector<double> getNumberVectorFromXMLRPC(robot_xmlrpcpp::XmlRpcValue& value)
 {
-  if (value.getType() != robot::XmlRpc::XmlRpcValue::TypeArray)
+  if (value.getType() != robot_xmlrpcpp::XmlRpcValue::TypeArray)
   {
     throw PolygonParseException("Subarray must have type list.");
   }
@@ -192,15 +192,15 @@ std::vector<double> getNumberVectorFromXMLRPC(robot::XmlRpc::XmlRpcValue& value)
   return array;
 }
 
-Polygon2D polygonFromXMLRPC(robot::XmlRpc::XmlRpcValue& polygon_xmlrpc)
+Polygon2D polygonFromXMLRPC(robot_xmlrpcpp::XmlRpcValue& polygon_xmlrpc)
 {
-  if (polygon_xmlrpc.getType() == robot::XmlRpc::XmlRpcValue::TypeString &&
+  if (polygon_xmlrpc.getType() == robot_xmlrpcpp::XmlRpcValue::TypeString &&
       polygon_xmlrpc != "" && polygon_xmlrpc != "[]")
   {
     return polygonFromString(std::string(polygon_xmlrpc));
   }
 
-  if (polygon_xmlrpc.getType() == robot::XmlRpc::XmlRpcValue::TypeStruct)
+  if (polygon_xmlrpc.getType() == robot_xmlrpcpp::XmlRpcValue::TypeStruct)
   {
     if (!polygon_xmlrpc.hasMember("x") || !polygon_xmlrpc.hasMember("y"))
     {
@@ -212,7 +212,7 @@ Polygon2D polygonFromXMLRPC(robot::XmlRpc::XmlRpcValue& polygon_xmlrpc)
   }
 
   // Make sure we have an array of at least 3 elements.
-  if (polygon_xmlrpc.getType() != robot::XmlRpc::XmlRpcValue::TypeArray)
+  if (polygon_xmlrpc.getType() != robot_xmlrpcpp::XmlRpcValue::TypeArray)
   {
     std::stringstream err_ss;
     err_ss << "Polygon must be specified as a list of lists. Found object of type " << polygon_xmlrpc.getType()
@@ -229,8 +229,8 @@ Polygon2D polygonFromXMLRPC(robot::XmlRpc::XmlRpcValue& polygon_xmlrpc)
   for (int i = 0; i < polygon_xmlrpc.size(); ++i)
   {
     // Make sure each element of the list is an array of size 2. (x and y coordinates)
-    robot::XmlRpc::XmlRpcValue& point_xml = polygon_xmlrpc[i];
-    if (point_xml.getType() != robot::XmlRpc::XmlRpcValue::TypeArray)
+    robot_xmlrpcpp::XmlRpcValue& point_xml = polygon_xmlrpc[i];
+    if (point_xml.getType() != robot_xmlrpcpp::XmlRpcValue::TypeArray)
     {
       std::stringstream err_ss;
       err_ss << "Each point must be specified as a list. Found object of type " << point_xml.getType() << " instead.";
@@ -274,9 +274,9 @@ Polygon2D polygonFromXMLRPC(robot::XmlRpc::XmlRpcValue& polygon_xmlrpc)
 /**
  * @brief Helper method to convert a vector of doubles
  */
-robot::XmlRpc::XmlRpcValue vectorToXMLRPC(const std::vector<double>& array)
+robot_xmlrpcpp::XmlRpcValue vectorToXMLRPC(const std::vector<double>& array)
 {
-  robot::XmlRpc::XmlRpcValue xml;
+  robot_xmlrpcpp::XmlRpcValue xml;
   xml.setSize(array.size());
   for (unsigned int i = 0; i < array.size(); ++i)
   {
@@ -285,9 +285,9 @@ robot::XmlRpc::XmlRpcValue vectorToXMLRPC(const std::vector<double>& array)
   return xml;
 }
 
-robot::XmlRpc::XmlRpcValue polygonToXMLRPC(const robot_nav_2d_msgs::Polygon2D& polygon, bool array_of_arrays)
+robot_xmlrpcpp::XmlRpcValue polygonToXMLRPC(const robot_nav_2d_msgs::Polygon2D& polygon, bool array_of_arrays)
 {
-  robot::XmlRpc::XmlRpcValue xml;
+  robot_xmlrpcpp::XmlRpcValue xml;
   if (array_of_arrays)
   {
     xml.setSize(polygon.points.size());
diff --git a/src/Libraries/xmlrpcpp b/src/Libraries/xmlrpcpp
index b647022..bb14979 160000
--- a/src/Libraries/xmlrpcpp
+++ b/src/Libraries/xmlrpcpp
@@ -1 +1 @@
-Subproject commit b64702260fc38013587ffa6c1c9c122b1d83de21
+Subproject commit bb14979b8a80b743a4dd41b43b5474d4c1a2fc3f