diff --git a/darwin_apps/launch/darwin_cpp.launch b/darwin_apps/launch/darwin_cpp.launch
index 0c37f2b2fda11eaa7cf276eb3a568c75fce9027a..0f015b348154730bb7770308df63f23065d99858 100644
--- a/darwin_apps/launch/darwin_cpp.launch
+++ b/darwin_apps/launch/darwin_cpp.launch
@@ -11,4 +11,15 @@
     <arg name="environment" value="$(arg environment)" />
   </include>
 
+  <node name="gazebo_image_to_video_dev"
+        pkg="gazebo_image_to_video_dev"
+        type="gazebo_image_to_video_dev"
+        output="screen">
+    <remap from="~/camera_in/image_raw"
+           to="/darwin/camera/image_raw"/>
+    <remap from="~/camera_in/camera_info"
+           to="/darwin/camera/camera_info"/>
+    <param name="video_dev" value="/dev/video0"/>
+  </node>
+
 </launch>
diff --git a/darwin_arm_kinematics/launch/left_arm_kin_test.launch b/darwin_arm_kinematics/launch/left_arm_kin_test.launch
index f399a016e0d2eca6f4d198b8fef8351745d2e91b..e75ab51f72ea32006662941bb51078dfb21a433a 100644
--- a/darwin_arm_kinematics/launch/left_arm_kin_test.launch
+++ b/darwin_arm_kinematics/launch/left_arm_kin_test.launch
@@ -4,7 +4,7 @@
 
   <!-- Convert an xacro and put on parameter server -->
   <param name="robot_description"
-         command="$(find xacro)/xacro.py '$(find darwin_description)/urdf/$(arg robot).xacro'" />
+         command="$(find xacro)/xacro --inorder '$(find darwin_description)/urdf/$(arg robot).xacro'" />
 
   <!-- load the controllers -->
   <node name="left_arm_kinematics" 
diff --git a/darwin_description/launch/charge_env.launch b/darwin_description/launch/charge_env.launch
index a6f67c3d7ae7672dabdcca65cc83eba95a5536bf..85eb98ed996d2ebdeb23357c7d98c193f2165ba9 100644
--- a/darwin_description/launch/charge_env.launch
+++ b/darwin_description/launch/charge_env.launch
@@ -4,7 +4,7 @@
   <group ns="$(arg environment)"> 
     <!-- Convert an xacro and put on parameter server -->
     <param name="robot_description"
-           command="$(find xacro)/xacro.py '$(find darwin_description)/urdf/charge/$(arg environment).xacro'" />
+           command="$(find xacro)/xacro --inorder '$(find darwin_description)/urdf/charge/$(arg environment).xacro'" />
 
 <!--    <node pkg="robot_state_publisher" type="state_publisher" name="env_tf_broadcaster">
       <param name="tf_prefix" type="string" value="/$(arg environment)"/>
diff --git a/darwin_description/launch/darwin_base.launch b/darwin_description/launch/darwin_base.launch
index fa75b58b38819bb6a68434f0cb86735cdf127c00..36cbd3d110b126c4ed21570cc64d644f3c07cf16 100644
--- a/darwin_description/launch/darwin_base.launch
+++ b/darwin_description/launch/darwin_base.launch
@@ -5,7 +5,7 @@
 
   <!-- Convert an xacro and put on parameter server -->
   <param name="robot_description"
-         command="$(find xacro)/xacro.py '$(find darwin_description)/urdf/$(arg robot).xacro'" />
+         command="$(find xacro)/xacro --inorder '$(find darwin_description)/urdf/$(arg robot).xacro'" />
 
   <node pkg="robot_state_publisher" type="state_publisher" name="darwin_tf_broadcaster">
     <param name="tf_prefix" type="string" value="/darwin"/>
diff --git a/darwin_description/launch/darwin_base_sim.launch b/darwin_description/launch/darwin_base_sim.launch
index 9d14760bf71bcae3d69ff8328363351bbea2ff99..7a2288bf912c3a39165c92724d9c1a06f560ab51 100644
--- a/darwin_description/launch/darwin_base_sim.launch
+++ b/darwin_description/launch/darwin_base_sim.launch
@@ -4,7 +4,7 @@
 
   <!-- Convert an xacro and put on parameter server -->
   <param name="robot_description"
-         command="$(find xacro)/xacro.py '$(find darwin_description)/urdf/$(arg robot).xacro'" />
+         command="$(find xacro)/xacro --inorder '$(find darwin_description)/urdf/$(arg robot).xacro'" />
 
   <node pkg="robot_state_publisher" type="state_publisher" name="darwin_tf_broadcaster">
     <param name="tf_prefix" type="string" value="/darwin"/>
diff --git a/darwin_description/launch/darwin_test.launch b/darwin_description/launch/darwin_test.launch
index 0e5370288f2c1581f866552410b4679385c15d67..dc97347bcc47cf80c0e0aef955078915a7e2fd86 100644
--- a/darwin_description/launch/darwin_test.launch
+++ b/darwin_description/launch/darwin_test.launch
@@ -4,7 +4,7 @@
 
   <!-- Convert an xacro and put on parameter server -->
   <param name="robot_description"
-         command="$(find xacro)/xacro.py '$(find darwin_description)/urdf/$(arg robot).xacro'" />
+         command="$(find xacro)/xacro --inorder '$(find darwin_description)/urdf/$(arg robot).xacro'" />
 
   <node pkg="robot_state_publisher" type="state_publisher" name="darwin_tf_broadcaster">
     <param name="tf_prefix" type="string" value="/darwin"/>
diff --git a/darwin_description/urdf/charge/charge_station.xacro b/darwin_description/urdf/charge/charge_station.xacro
index ad70631fa19527f244102e228c68e087816beb63..d966460c2e4afeacaa5a86086dacf06d564b6ea5 100644
--- a/darwin_description/urdf/charge/charge_station.xacro
+++ b/darwin_description/urdf/charge/charge_station.xacro
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 
-<root xmlns:xacro="http://ros.org/wiki/xacro">
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
 
   <xacro:include filename="$(find bioloid_description)/urdf/qrcodes/qrcode.xacro" />
 
diff --git a/darwin_description/urdf/gripper.gazebo b/darwin_description/urdf/gripper.gazebo
index b935a753ab4c18e766e87c3c327df6090a142be9..4fac2a895bdc5d810e965f2242b3d11b75817912 100644
--- a/darwin_description/urdf/gripper.gazebo
+++ b/darwin_description/urdf/gripper.gazebo
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 
-<root xmlns:xacro="http://ros.org/wiki/xacro">
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
 
   <xacro:macro name="gripper_gazebo" params="name">
     <gazebo reference="${name}_elbow">
diff --git a/darwin_description/urdf/gripper.xacro b/darwin_description/urdf/gripper.xacro
index a2215018374edb0eaccdc62f587b6ef84b0a1353..0a520867d76caea106303272fa131367bb5f5544 100755
--- a/darwin_description/urdf/gripper.xacro
+++ b/darwin_description/urdf/gripper.xacro
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 
-<root xmlns:xacro="http://ros.org/wiki/xacro">
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
 
   <xacro:include filename="$(find darwin_description)/urdf/gripper.gazebo" />
 
diff --git a/darwin_description/urdf/gripper_sc.gazebo b/darwin_description/urdf/gripper_sc.gazebo
index c6b54d23aaf68936b070a2628585ceefe938fa37..8265ba7c4357aee80e9bc1d453a73d724a31e7a9 100644
--- a/darwin_description/urdf/gripper_sc.gazebo
+++ b/darwin_description/urdf/gripper_sc.gazebo
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 
-<root xmlns:xacro="http://ros.org/wiki/xacro">
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
 
   <xacro:macro name="gripper_sc_gazebo" params="name">
     <gazebo reference="${name}_elbow">
diff --git a/darwin_description/urdf/gripper_sc.xacro b/darwin_description/urdf/gripper_sc.xacro
index 5a5269f436c8da20a6ed6557c9be6afbb7fe1922..60d41509163efb3934c841a2b13b1fa7ea1733f2 100755
--- a/darwin_description/urdf/gripper_sc.xacro
+++ b/darwin_description/urdf/gripper_sc.xacro
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 
-<root xmlns:xacro="http://ros.org/wiki/xacro">
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
 
   <xacro:include filename="$(find darwin_description)/urdf/gripper_sc.gazebo" />
 
diff --git a/darwin_description/urdf/gripper_sc_low_res.xacro b/darwin_description/urdf/gripper_sc_low_res.xacro
index 52838e437cefa95d67b0743457012e58c3f4113c..5bf5cd21b99923834badeb4a6b82f79b7fede153 100755
--- a/darwin_description/urdf/gripper_sc_low_res.xacro
+++ b/darwin_description/urdf/gripper_sc_low_res.xacro
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 
-<root xmlns:xacro="http://ros.org/wiki/xacro">
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
 
   <xacro:include filename="$(find darwin_description)/urdf/gripper_sc.gazebo" />
 
diff --git a/darwin_description/urdf/left_foot.xacro b/darwin_description/urdf/left_foot.xacro
index d6e64871294cfbf05dd01074c96655266c8bdda0..67668eabc3bcaed97008db0953bb935a4829da14 100755
--- a/darwin_description/urdf/left_foot.xacro
+++ b/darwin_description/urdf/left_foot.xacro
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 
-<root xmlns:xacro="http://ros.org/wiki/xacro">
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
 
   <xacro:include filename="$(find darwin_description)/urdf/left_foot.gazebo" />
 
diff --git a/darwin_description/urdf/left_hand.gazebo b/darwin_description/urdf/left_hand.gazebo
index bd86a71218430dcd94b7a79338f8e1e26a571d58..2077a54e245fd734df764e1c2e3853aba7edd6d2 100644
--- a/darwin_description/urdf/left_hand.gazebo
+++ b/darwin_description/urdf/left_hand.gazebo
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 
-<root xmlns:xacro="http://ros.org/wiki/xacro">
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
 
   <gazebo reference="left_arm_low">
     <material>Gazebo/Grey</material>
diff --git a/darwin_description/urdf/left_hand.xacro b/darwin_description/urdf/left_hand.xacro
index 9dfa4beea73ef1717a2380bd8eb6e73c5f2ffffd..3d6f7cf53564229510c565e9e2d11f344a77ee8f 100755
--- a/darwin_description/urdf/left_hand.xacro
+++ b/darwin_description/urdf/left_hand.xacro
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 
-<root xmlns:xacro="http://ros.org/wiki/xacro">
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
 
   <xacro:include filename="$(find darwin_description)/urdf/left_hand.gazebo" />
 
diff --git a/darwin_description/urdf/left_sensor_foot.xacro b/darwin_description/urdf/left_sensor_foot.xacro
index 45fb6d9f1bd92748f9fa2822937232a351fc2d46..fd902b7f472ac91221c80c0586b78b0874634168 100755
--- a/darwin_description/urdf/left_sensor_foot.xacro
+++ b/darwin_description/urdf/left_sensor_foot.xacro
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 
-<root xmlns:xacro="http://ros.org/wiki/xacro">
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
 
   <xacro:include filename="$(find darwin_description)/urdf/left_foot.gazebo" />
   <xacro:include filename="$(find bioloid_description)/urdf/sensors/cny70_ir.xacro" />
diff --git a/darwin_description/urdf/right_foot.xacro b/darwin_description/urdf/right_foot.xacro
index bdd8a1b78e5f8430f6e8fd4d582a2372241adc27..eb12dc8577a4829042282979cfeb138c606e37df 100755
--- a/darwin_description/urdf/right_foot.xacro
+++ b/darwin_description/urdf/right_foot.xacro
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 
-<root xmlns:xacro="http://ros.org/wiki/xacro">
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
 
   <xacro:include filename="$(find darwin_description)/urdf/right_foot.gazebo" />
 
diff --git a/darwin_description/urdf/right_hand.gazebo b/darwin_description/urdf/right_hand.gazebo
index 55df05a9462bca7bc3ffbd1f99f9431e55b707f2..796338f0f2b77497d05e65373b9a39d916c1c3f6 100644
--- a/darwin_description/urdf/right_hand.gazebo
+++ b/darwin_description/urdf/right_hand.gazebo
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 
-<root xmlns:xacro="http://ros.org/wiki/xacro">
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
 
   <gazebo reference="right_arm_low">
     <material>Gazebo/Grey</material>
diff --git a/darwin_description/urdf/right_hand.xacro b/darwin_description/urdf/right_hand.xacro
index 8c06c5d1f4ce5aec3e365a820eda031d48ed1bbb..ec48b6430611d2a3eaebc50a229ca47d2d7a5164 100755
--- a/darwin_description/urdf/right_hand.xacro
+++ b/darwin_description/urdf/right_hand.xacro
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 
-<root xmlns:xacro="http://ros.org/wiki/xacro">
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
 
   <xacro:include filename="$(find darwin_description)/urdf/right_hand.gazebo" />
 
diff --git a/darwin_description/urdf/right_sensor_foot.xacro b/darwin_description/urdf/right_sensor_foot.xacro
index a8ab93be0da60fb89708d796be4226493d1797fb..f13e4a51e1019e0c66a079008e894d05464a4a82 100755
--- a/darwin_description/urdf/right_sensor_foot.xacro
+++ b/darwin_description/urdf/right_sensor_foot.xacro
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 
-<root xmlns:xacro="http://ros.org/wiki/xacro">
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
 
   <xacro:include filename="$(find darwin_description)/urdf/right_foot.gazebo" />
   <xacro:include filename="$(find bioloid_description)/urdf/sensors/cny70_ir.xacro" />
diff --git a/darwin_description/urdf/sensors/bno055.gazebo b/darwin_description/urdf/sensors/bno055.gazebo
index 709e0cb509bd26fe676d7f3e2e535d5429e9dc5b..3f4045446d057dc63eb77eeff67d100502102b44 100644
--- a/darwin_description/urdf/sensors/bno055.gazebo
+++ b/darwin_description/urdf/sensors/bno055.gazebo
@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<root xmlns:xacro="http://ros.org/wiki/xacro">
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
   <xacro:macro name="bno055_gazebo" params="name">
     <gazebo>
       <plugin name="bno055_sim" filename="libhector_gazebo_ros_imu.so">
diff --git a/darwin_description/urdf/sensors/ids_xs.gazebo b/darwin_description/urdf/sensors/ids_xs.gazebo
index 5425e6d9aad163ec18993d88e82f08d543b8f3a0..787dec90e12c11214cbc44aa54f595364c2422a0 100644
--- a/darwin_description/urdf/sensors/ids_xs.gazebo
+++ b/darwin_description/urdf/sensors/ids_xs.gazebo
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 
-<root xmlns:xacro="http://ros.org/wiki/xacro">
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
 
   <xacro:macro name="ids_xs_gazebo" params="name">
     <gazebo reference="${name}_link">
diff --git a/darwin_description/urdf/sensors/ids_xs.xacro b/darwin_description/urdf/sensors/ids_xs.xacro
index daeefd6003a78356c260c65a1ec8a7c118eb606d..e07bf5b471874cffe5521495246e1fa4098e7fc9 100644
--- a/darwin_description/urdf/sensors/ids_xs.xacro
+++ b/darwin_description/urdf/sensors/ids_xs.xacro
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 
-<root xmlns:xacro="http://ros.org/wiki/xacro">
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
 
   <xacro:include filename="$(find darwin_description)/urdf/sensors/ids_xs.gazebo" />
 
diff --git a/darwin_driver/CMakeLists.txt b/darwin_driver/CMakeLists.txt
index aceb2582a2afb2a25239db1a53ef5f3b7c78ee25..c6b2276df8a5aa302c5516385b1f935add38d2a8 100644
--- a/darwin_driver/CMakeLists.txt
+++ b/darwin_driver/CMakeLists.txt
@@ -100,6 +100,8 @@ target_link_libraries(${PROJECT_NAME} ${darwin_robot_LIBRARY})
 #               Add message headers dependencies 
 # ******************************************************************** 
 # add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} iri_base_driver_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME}_gencfg iri_base_driver_generate_messages_cpp)
 add_dependencies(${PROJECT_NAME} std_msgs_generate_messages_cpp)
 add_dependencies(${PROJECT_NAME} control_msgs_generate_messages_cpp)
 add_dependencies(${PROJECT_NAME} trajectory_msgs_generate_messages_cpp)
diff --git a/darwin_driver/cfg/DarwinDriver.cfg b/darwin_driver/cfg/DarwinDriver.cfg
index 6091edae554a77bdeafcc723bb9a44247501bdf3..57d60b368d72e4ba3723cbe13934796456c27367 100755
--- a/darwin_driver/cfg/DarwinDriver.cfg
+++ b/darwin_driver/cfg/DarwinDriver.cfg
@@ -33,7 +33,7 @@
 
 PACKAGE='darwin_driver'
 
-from driver_base.msg import SensorLevels
+from iri_base_driver.msg import SensorLevels
 from dynamic_reconfigure.parameter_generator_catkin import *
 
 gen = ParameterGenerator()
diff --git a/darwin_driver/src/darwin_driver.cpp b/darwin_driver/src/darwin_driver.cpp
index 98c51a2c711991c79b711b00bce73ab0bb0d7ea2..c23b0f72923bc7db2730177a2f299c9efa716ebe 100644
--- a/darwin_driver/src/darwin_driver.cpp
+++ b/darwin_driver/src/darwin_driver.cpp
@@ -189,13 +189,13 @@ void DarwinDriver::config_update(Config& new_cfg, uint32_t level)
   // update driver with new_cfg data
   switch(this->getState())
   {
-    case DarwinDriver::CLOSED:
+    case iri_base_driver::CLOSED:
       break;
 
-    case DarwinDriver::OPENED:
+    case iri_base_driver::OPENED:
       break;
 
-    case DarwinDriver::RUNNING:
+    case iri_base_driver::RUNNING:
       break;
   }
 
diff --git a/darwin_driver/src/darwin_driver_node.cpp b/darwin_driver/src/darwin_driver_node.cpp
index c086f9210406c08d9677bb4698fe2937cdd286c5..37803115b36805945e291cd5ed89dc4ac4fb182a 100644
--- a/darwin_driver/src/darwin_driver_node.cpp
+++ b/darwin_driver/src/darwin_driver_node.cpp
@@ -1334,5 +1334,5 @@ DarwinDriverNode::~DarwinDriverNode(void)
 /* main function */
 int main(int argc,char *argv[])
 {
-  return driver_base::main<DarwinDriverNode>(argc, argv, "darwin_driver_node");
+  return iri_base_driver::main<DarwinDriverNode>(argc, argv, "darwin_driver_node");
 }
diff --git a/darwin_robot/package.xml b/darwin_robot/package.xml
index b26048bb3f65d0d0643f93de6b76efadf4a6c70a..cf8a634a9a3f460562ee83ce8ae070d448745b02 100644
--- a/darwin_robot/package.xml
+++ b/darwin_robot/package.xml
@@ -39,6 +39,7 @@
   <!--   <run_depend>message_runtime</run_depend> -->
   <run_depend>darwin_control</run_depend>
   <run_depend>darwin_controller</run_depend>
+  <run_depend>darwin_controller_cpp</run_depend>
   <run_depend>darwin_description</run_depend>
   <run_depend>darwin_gazebo</run_depend>
   <run_depend>darwin_driver</run_depend>