From 90698b66c9e92efc0e9f5c3bd7a7f5bc6f3d4893 Mon Sep 17 00:00:00 2001
From: IvanSnap360 <ivpetr2003@gmail.com>
Date: Sat, 3 Jun 2023 20:26:37 +0300
Subject: [PATCH 01/13] change battery controller firmware

---
 .../battery_controller_firmware_V1.ino        |   11 +-
 .../battery_controller_firmware_V1/config.h   |    4 +-
 src/omnibot_bringup/rviz/rviz.rviz            | 1221 +----------------
 3 files changed, 51 insertions(+), 1185 deletions(-)

diff --git a/firmware/battery_controller_firmware_V1/battery_controller_firmware_V1.ino b/firmware/battery_controller_firmware_V1/battery_controller_firmware_V1.ino
index 9823bfd..54524a3 100644
--- a/firmware/battery_controller_firmware_V1/battery_controller_firmware_V1.ino
+++ b/firmware/battery_controller_firmware_V1/battery_controller_firmware_V1.ino
@@ -25,12 +25,12 @@ void loop()
   if (millis() - send_lt > (uint32_t)(1000.0 / SERIAL_SEND_RATE_HZ))
   {
     battery[BATTERY_1][CELL_1] = getVoltsFromPin(battery_1_cell_pins[CELL_1]);
-    battery[BATTERY_1][CELL_2] = getVoltsFromPin(battery_1_cell_pins[CELL_2]) - battery[BATTERY_1][CELL_1];
-    battery[BATTERY_1][CELL_3] = getVoltsFromPin(battery_1_cell_pins[CELL_3]) - (battery[BATTERY_1][CELL_1] + battery[BATTERY_1][CELL_2]);
+    battery[BATTERY_1][CELL_2] = getVoltsFromPin(battery_1_cell_pins[CELL_2]);// - battery[BATTERY_1][CELL_1];
+    battery[BATTERY_1][CELL_3] = getVoltsFromPin(battery_1_cell_pins[CELL_3]);// - (battery[BATTERY_1][CELL_1] + battery[BATTERY_1][CELL_2]);
 
     battery[BATTERY_2][CELL_1] = getVoltsFromPin(battery_2_cell_pins[CELL_1]);
-    battery[BATTERY_2][CELL_2] = getVoltsFromPin(battery_2_cell_pins[CELL_2]) - battery[BATTERY_2][CELL_1];
-    battery[BATTERY_2][CELL_3] = getVoltsFromPin(battery_2_cell_pins[CELL_3]) - (battery[BATTERY_2][CELL_1] + battery[BATTERY_2][CELL_2]);
+    battery[BATTERY_2][CELL_2] = getVoltsFromPin(battery_2_cell_pins[CELL_2]);// - battery[BATTERY_2][CELL_1];
+    battery[BATTERY_2][CELL_3] = getVoltsFromPin(battery_2_cell_pins[CELL_3]);//- (battery[BATTERY_2][CELL_1] + battery[BATTERY_2][CELL_2]);
     print_values();
     send_lt = millis();
   }
@@ -67,7 +67,8 @@ float getVoltsFromPin(int pin)
 {
   float vin = 0.0;
   vin = ((analogRead(pin) * SOURCE_VOLTAGE_VOLTS) / ADC_MAX_VALUE) / (R2 / (R1 + R2));
-  if (vin < 0.1)
+//  vin = analogRead(pin);/
+  if (vin < 0.3)
     vin = 0.0;
 
   return abs(vin);
diff --git a/firmware/battery_controller_firmware_V1/config.h b/firmware/battery_controller_firmware_V1/config.h
index adfc00d..814a211 100644
--- a/firmware/battery_controller_firmware_V1/config.h
+++ b/firmware/battery_controller_firmware_V1/config.h
@@ -32,8 +32,8 @@ enum
 #define ON HIGH
 
 
-#define RESITOR_DIVIDER__R1_VAL_Om (float)2200.0
-#define RESITOR_DIVIDER__R2_VAL_Om (float)1000.0
+#define RESITOR_DIVIDER__R1_VAL_Om (float)100000.0
+#define RESITOR_DIVIDER__R2_VAL_Om (float)10000.0
 #define ADC_MAX_VALUE (float)1024.0
 #define SOURCE_VOLTAGE_VOLTS (float)4.6
 
diff --git a/src/omnibot_bringup/rviz/rviz.rviz b/src/omnibot_bringup/rviz/rviz.rviz
index 20f8eda..8d9f466 100644
--- a/src/omnibot_bringup/rviz/rviz.rviz
+++ b/src/omnibot_bringup/rviz/rviz.rviz
@@ -12,7 +12,7 @@ Panels:
         - /move_base1/RRT_Global_Planner1/global_plan1
         - /move_base1/EbandPlanner1
       Splitter Ratio: 0.5080732107162476
-    Tree Height: 758
+    Tree Height: 715
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -65,24 +65,14 @@ Visualization Manager:
         All Enabled: true
         ang_link0:
           Value: true
-        ang_link1:
-          Value: true
-        ang_link2:
-          Value: true
-        ang_link3:
-          Value: true
-        ang_link4:
-          Value: true
         ang_link5:
           Value: true
-        ang_link6:
-          Value: true
-        ang_link7:
-          Value: true
         base_footprint:
           Value: true
         base_link:
           Value: true
+        d400_aligned_depth_to_color_frame:
+          Value: true
         d400_bottom_screw_frame:
           Value: true
         d400_color_frame:
@@ -93,6 +83,14 @@ Visualization Manager:
           Value: true
         d400_depth_optical_frame:
           Value: true
+        d400_infra1_frame:
+          Value: true
+        d400_infra1_optical_frame:
+          Value: true
+        d400_infra2_frame:
+          Value: true
+        d400_infra2_optical_frame:
+          Value: true
         d400_left_ir_frame:
           Value: true
         d400_left_ir_optical_frame:
@@ -107,66 +105,10 @@ Visualization Manager:
           Value: true
         left_back_mecanum_wheel_link:
           Value: true
-        left_back_roller_1_1_link:
-          Value: true
-        left_back_roller_1_2_link:
-          Value: true
-        left_back_roller_1_3_link:
-          Value: true
-        left_back_roller_1_4_link:
-          Value: true
-        left_back_roller_1_5_link:
-          Value: true
-        left_back_roller_1_6_link:
-          Value: true
-        left_back_roller_1_7_link:
-          Value: true
-        left_back_roller_2_1_link:
-          Value: true
-        left_back_roller_2_2_link:
-          Value: true
-        left_back_roller_2_3_link:
-          Value: true
-        left_back_roller_2_4_link:
-          Value: true
-        left_back_roller_2_5_link:
-          Value: true
-        left_back_roller_2_6_link:
-          Value: true
-        left_back_roller_2_7_link:
-          Value: true
         left_front_mecanum_adapter_link:
           Value: true
         left_front_mecanum_wheel_link:
           Value: true
-        left_front_roller_1_1_link:
-          Value: true
-        left_front_roller_1_2_link:
-          Value: true
-        left_front_roller_1_3_link:
-          Value: true
-        left_front_roller_1_4_link:
-          Value: true
-        left_front_roller_1_5_link:
-          Value: true
-        left_front_roller_1_6_link:
-          Value: true
-        left_front_roller_1_7_link:
-          Value: true
-        left_front_roller_2_1_link:
-          Value: true
-        left_front_roller_2_2_link:
-          Value: true
-        left_front_roller_2_3_link:
-          Value: true
-        left_front_roller_2_4_link:
-          Value: true
-        left_front_roller_2_5_link:
-          Value: true
-        left_front_roller_2_6_link:
-          Value: true
-        left_front_roller_2_7_link:
-          Value: true
         map:
           Value: true
         odom:
@@ -175,66 +117,10 @@ Visualization Manager:
           Value: true
         right_back_mecanum_wheel_link:
           Value: true
-        right_back_roller_1_1_link:
-          Value: true
-        right_back_roller_1_2_link:
-          Value: true
-        right_back_roller_1_3_link:
-          Value: true
-        right_back_roller_1_4_link:
-          Value: true
-        right_back_roller_1_5_link:
-          Value: true
-        right_back_roller_1_6_link:
-          Value: true
-        right_back_roller_1_7_link:
-          Value: true
-        right_back_roller_2_1_link:
-          Value: true
-        right_back_roller_2_2_link:
-          Value: true
-        right_back_roller_2_3_link:
-          Value: true
-        right_back_roller_2_4_link:
-          Value: true
-        right_back_roller_2_5_link:
-          Value: true
-        right_back_roller_2_6_link:
-          Value: true
-        right_back_roller_2_7_link:
-          Value: true
         right_front_mecanum_adapter_link:
           Value: true
         right_front_mecanum_wheel_link:
           Value: true
-        right_front_roller_1_1_link:
-          Value: true
-        right_front_roller_1_2_link:
-          Value: true
-        right_front_roller_1_3_link:
-          Value: true
-        right_front_roller_1_4_link:
-          Value: true
-        right_front_roller_1_5_link:
-          Value: true
-        right_front_roller_1_6_link:
-          Value: true
-        right_front_roller_1_7_link:
-          Value: true
-        right_front_roller_2_1_link:
-          Value: true
-        right_front_roller_2_2_link:
-          Value: true
-        right_front_roller_2_3_link:
-          Value: true
-        right_front_roller_2_4_link:
-          Value: true
-        right_front_roller_2_5_link:
-          Value: true
-        right_front_roller_2_6_link:
-          Value: true
-        right_front_roller_2_7_link:
-          Value: true
         t265_accel_frame:
           Value: true
         t265_accel_optical_frame:
@@ -264,24 +150,25 @@ Visualization Manager:
       Show Axes: true
       Show Names: true
       Tree:
+        ang_link5:
+          tip_link:
+            {}
+        left_back_mecanum_wheel_link:
+          left_back_mecanum_adapter_link:
+            {}
+        left_front_mecanum_wheel_link:
+          left_front_mecanum_adapter_link:
+            {}
         map:
           odom:
             base_footprint:
               ang_link0:
-                ang_link1:
-                  ang_link2:
-                    ang_link3:
-                      ang_link4:
-                        ang_link5:
-                          ang_link6:
-                            {}
-                          ang_link7:
-                            {}
-                          tip_link:
-                            {}
+                {}
               base_link:
                 d400_bottom_screw_frame:
                   d400_link:
+                    d400_aligned_depth_to_color_frame:
+                      {}
                     d400_depth_frame:
                       d400_color_frame:
                         d400_color_optical_frame:
@@ -294,130 +181,12 @@ Visualization Manager:
                       d400_right_ir_frame:
                         d400_right_ir_optical_frame:
                           {}
-                left_back_mecanum_wheel_link:
-                  left_back_mecanum_adapter_link:
-                    {}
-                  left_back_roller_1_1_link:
-                    {}
-                  left_back_roller_1_2_link:
-                    {}
-                  left_back_roller_1_3_link:
-                    {}
-                  left_back_roller_1_4_link:
-                    {}
-                  left_back_roller_1_5_link:
-                    {}
-                  left_back_roller_1_6_link:
-                    {}
-                  left_back_roller_1_7_link:
-                    {}
-                  left_back_roller_2_1_link:
-                    {}
-                  left_back_roller_2_2_link:
-                    {}
-                  left_back_roller_2_3_link:
-                    {}
-                  left_back_roller_2_4_link:
-                    {}
-                  left_back_roller_2_5_link:
-                    {}
-                  left_back_roller_2_6_link:
-                    {}
-                  left_back_roller_2_7_link:
-                    {}
-                left_front_mecanum_wheel_link:
-                  left_front_mecanum_adapter_link:
-                    {}
-                  left_front_roller_1_1_link:
-                    {}
-                  left_front_roller_1_2_link:
-                    {}
-                  left_front_roller_1_3_link:
-                    {}
-                  left_front_roller_1_4_link:
-                    {}
-                  left_front_roller_1_5_link:
-                    {}
-                  left_front_roller_1_6_link:
-                    {}
-                  left_front_roller_1_7_link:
-                    {}
-                  left_front_roller_2_1_link:
-                    {}
-                  left_front_roller_2_2_link:
-                    {}
-                  left_front_roller_2_3_link:
-                    {}
-                  left_front_roller_2_4_link:
-                    {}
-                  left_front_roller_2_5_link:
-                    {}
-                  left_front_roller_2_6_link:
-                    {}
-                  left_front_roller_2_7_link:
-                    {}
-                right_back_mecanum_wheel_link:
-                  right_back_mecanum_adapter_link:
-                    {}
-                  right_back_roller_1_1_link:
-                    {}
-                  right_back_roller_1_2_link:
-                    {}
-                  right_back_roller_1_3_link:
-                    {}
-                  right_back_roller_1_4_link:
-                    {}
-                  right_back_roller_1_5_link:
-                    {}
-                  right_back_roller_1_6_link:
-                    {}
-                  right_back_roller_1_7_link:
-                    {}
-                  right_back_roller_2_1_link:
-                    {}
-                  right_back_roller_2_2_link:
-                    {}
-                  right_back_roller_2_3_link:
-                    {}
-                  right_back_roller_2_4_link:
-                    {}
-                  right_back_roller_2_5_link:
-                    {}
-                  right_back_roller_2_6_link:
-                    {}
-                  right_back_roller_2_7_link:
-                    {}
-                right_front_mecanum_wheel_link:
-                  right_front_mecanum_adapter_link:
-                    {}
-                  right_front_roller_1_1_link:
-                    {}
-                  right_front_roller_1_2_link:
-                    {}
-                  right_front_roller_1_3_link:
-                    {}
-                  right_front_roller_1_4_link:
-                    {}
-                  right_front_roller_1_5_link:
-                    {}
-                  right_front_roller_1_6_link:
-                    {}
-                  right_front_roller_1_7_link:
-                    {}
-                  right_front_roller_2_1_link:
-                    {}
-                  right_front_roller_2_2_link:
-                    {}
-                  right_front_roller_2_3_link:
-                    {}
-                  right_front_roller_2_4_link:
-                    {}
-                  right_front_roller_2_5_link:
-                    {}
-                  right_front_roller_2_6_link:
-                    {}
-                  right_front_roller_2_7_link:
-                    {}
+                    d400_infra1_frame:
+                      d400_infra1_optical_frame:
+                        {}
+                    d400_infra2_frame:
+                      d400_infra2_optical_frame:
+                        {}
                 t265_pose_frame:
                   t265_link:
                     t265_accel_frame:
@@ -432,6 +201,12 @@ Visualization Manager:
                     t265_gyro_frame:
                       t265_gyro_optical_frame:
                         {}
+        right_back_mecanum_wheel_link:
+          right_back_mecanum_adapter_link:
+            {}
+        right_front_mecanum_wheel_link:
+          right_front_mecanum_adapter_link:
+            {}
       Update Interval: 0
       Value: true
     - Alpha: 1
@@ -446,7 +221,7 @@ Visualization Manager:
       Color: 255; 255; 255
       Color Transformer: RGB8
       Decay Time: 0
-      Enabled: true
+      Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Min Color: 0; 0; 0
@@ -461,7 +236,7 @@ Visualization Manager:
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
-      Value: true
+      Value: false
     - Alpha: 0.699999988079071
       Class: rviz/Map
       Color Scheme: map
@@ -1539,461 +1314,6 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
-          ang_link0:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          ang_link1:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          ang_link2:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          ang_link3:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          ang_link4:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          ang_link5:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          ang_link6:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          ang_link7:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          base_footprint:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          base_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          d400_bottom_screw_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          d400_color_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          d400_color_optical_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          d400_depth_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          d400_depth_optical_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          d400_left_ir_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          d400_left_ir_optical_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          d400_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          d400_right_ir_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          d400_right_ir_optical_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          left_back_mecanum_adapter_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_mecanum_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_1_1_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_1_2_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_1_3_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_1_4_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_1_5_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_1_6_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_1_7_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_2_1_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_2_2_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_2_3_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_2_4_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_2_5_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_2_6_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_2_7_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_mecanum_adapter_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_mecanum_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_1_1_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_1_2_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_1_3_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_1_4_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_1_5_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_1_6_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_1_7_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_2_1_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_2_2_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_2_3_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_2_4_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_2_5_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_2_6_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_2_7_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_mecanum_adapter_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_mecanum_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_1_1_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_1_2_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_1_3_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_1_4_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_1_5_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_1_6_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_1_7_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_2_1_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_2_2_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_2_3_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_2_4_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_2_5_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_2_6_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_2_7_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_mecanum_adapter_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_mecanum_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_1_1_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_1_2_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_1_3_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_1_4_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_1_5_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_1_6_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_1_7_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_2_1_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_2_2_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_2_3_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_2_4_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_2_5_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_2_6_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_2_7_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          t265_accel_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          t265_accel_optical_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          t265_fisheye1_optical_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          t265_fisheye1_rgb_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          t265_fisheye2_optical_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          t265_fisheye2_rgb_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          t265_gyro_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          t265_gyro_optical_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          t265_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          t265_pose_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
         Loop Animation: false
         Robot Alpha: 0.5
         Robot Color: 150; 50; 150
@@ -2040,461 +1360,6 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
-          ang_link0:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          ang_link1:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          ang_link2:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          ang_link3:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          ang_link4:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          ang_link5:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          ang_link6:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          ang_link7:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          base_footprint:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          base_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          d400_bottom_screw_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          d400_color_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          d400_color_optical_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          d400_depth_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          d400_depth_optical_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          d400_left_ir_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          d400_left_ir_optical_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          d400_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          d400_right_ir_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          d400_right_ir_optical_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          left_back_mecanum_adapter_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_mecanum_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_1_1_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_1_2_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_1_3_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_1_4_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_1_5_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_1_6_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_1_7_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_2_1_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_2_2_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_2_3_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_2_4_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_2_5_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_2_6_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_roller_2_7_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_mecanum_adapter_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_mecanum_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_1_1_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_1_2_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_1_3_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_1_4_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_1_5_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_1_6_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_1_7_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_2_1_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_2_2_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_2_3_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_2_4_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_2_5_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_2_6_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_roller_2_7_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_mecanum_adapter_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_mecanum_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_1_1_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_1_2_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_1_3_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_1_4_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_1_5_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_1_6_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_1_7_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_2_1_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_2_2_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_2_3_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_2_4_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_2_5_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_2_6_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_roller_2_7_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_mecanum_adapter_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_mecanum_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_1_1_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_1_2_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_1_3_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_1_4_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_1_5_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_1_6_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_1_7_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_2_1_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_2_2_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_2_3_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_2_4_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_2_5_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_2_6_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_roller_2_7_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          t265_accel_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          t265_accel_optical_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          t265_fisheye1_optical_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          t265_fisheye1_rgb_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          t265_fisheye2_optical_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          t265_fisheye2_rgb_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          t265_gyro_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          t265_gyro_optical_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          t265_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          t265_pose_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
         Robot Alpha: 1
         Show Robot Collision: false
         Show Robot Visual: false
@@ -2544,14 +1409,14 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.8103974461555481
+      Pitch: 0.5653976798057556
       Target Frame: base_link
-      Yaw: 1.2335915565490723
+      Yaw: 2.9935927391052246
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 2086
+  Height: 2002
   Hide Left Dock: false
   Hide Right Dock: false
   Image:
@@ -2560,7 +1425,7 @@ Window Geometry:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
@@ -2569,6 +1434,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 3696
+  Width: 3264
   X: 144
-  Y: 0
+  Y: 54

From a966a94ee23e34e3772d12f502922d33c3d724f4 Mon Sep 17 00:00:00 2001
From: IvanSnap360 <ivpetr2003@gmail.com>
Date: Sat, 3 Jun 2023 21:03:26 +0300
Subject: [PATCH 02/13] some rtabmap changes

---
 platform_teleop.sh                            |   1 +
 .../launch/omnibot_gazebo.launch              |  10 +-
 .../launch/explore.launch                     |   2 +-
 .../launch/rtabmap.launch                     | 185 ++++++++++--------
 4 files changed, 105 insertions(+), 93 deletions(-)

diff --git a/platform_teleop.sh b/platform_teleop.sh
index 57c9141..8736ce6 100644
--- a/platform_teleop.sh
+++ b/platform_teleop.sh
@@ -1,2 +1,3 @@
+source_ws
 roslaunch omnibot_platform_teleop omnibot_platform_teleop.launch control_keys_variant:=numpad
 
diff --git a/src/omnibot_gazebo/launch/omnibot_gazebo.launch b/src/omnibot_gazebo/launch/omnibot_gazebo.launch
index 872c493..ad36b3e 100644
--- a/src/omnibot_gazebo/launch/omnibot_gazebo.launch
+++ b/src/omnibot_gazebo/launch/omnibot_gazebo.launch
@@ -30,18 +30,18 @@
   <arg name="z_pos" default="0.3"/> -->
 
   <!-- MyWorld -->
-  <!-- <arg name="x_pos" default="-4.0"/>
+  <arg name="x_pos" default="-4.0"/>
   <arg name="y_pos" default="3.0"/>
-  <arg name="z_pos" default="0.3"/> -->
+  <arg name="z_pos" default="0.3"/>
   <!-- 
   <arg name="x_pos" default="0.0"/>
   <arg name="y_pos" default="-2.65"/>
   <arg name="z_pos" default="0.3"/> -->
 
-
+<!-- 
   <arg name="x_pos" default="0.0" />
   <arg name="y_pos" default="1.0" />
-  <arg name="z_pos" default="0.3" />
+  <arg name="z_pos" default="0.3" /> -->
 
 
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
@@ -65,7 +65,7 @@
     <!-- <arg name="world_name" value="$(find
     omnibot_gazebo)/worlds/QuickSilverWorld/quicksilver_world_design1.world" /> -->
     <!-- <arg name="world_name" value="$(find omnibot_gazebo)/worlds/garage.world" /> -->
-    <arg name="world_name" value="$(find omnibot_gazebo)/worlds/MyWorld/MyWorldDyn.world" />
+    <arg name="world_name" value="$(find omnibot_gazebo)/worlds/MyWorld/MyWorld.world" />
 
   </include>
 
diff --git a/src/omnibot_platform/omnibot_platform_explorer/launch/explore.launch b/src/omnibot_platform/omnibot_platform_explorer/launch/explore.launch
index 5d9c348..25a85e1 100644
--- a/src/omnibot_platform/omnibot_platform_explorer/launch/explore.launch
+++ b/src/omnibot_platform/omnibot_platform_explorer/launch/explore.launch
@@ -1,7 +1,7 @@
 <launch>
 <node pkg="explore_lite" type="explore" respawn="false" name="explore" output="screen">
   <param name="robot_base_frame" value="base_footprint"/>
-  <param name="costmap_topic" value="/rtabmap/grid_map"/>
+  <param name="costmap_topic" value="map"/>
   <param name="costmap_updates_topic" value="map_updates"/>
   <param name="visualize" value="true"/>
   <param name="planner_frequency" value="3.5"/>
diff --git a/src/omnibot_platform/omnibot_platform_navigation/launch/rtabmap.launch b/src/omnibot_platform/omnibot_platform_navigation/launch/rtabmap.launch
index adfb0ce..6fc1caa 100644
--- a/src/omnibot_platform/omnibot_platform_navigation/launch/rtabmap.launch
+++ b/src/omnibot_platform/omnibot_platform_navigation/launch/rtabmap.launch
@@ -9,9 +9,9 @@
 <arg name="wait_for_transform" default="0.2" />
 <arg name="sim" default="false"/>
 <!--  -->
-<!-- <arg name="world_db_name" default="my_world_db2.db" if="$(arg sim)"/> -->
+<arg name="world_db_name" default="my_world_db2.db" if="$(arg sim)"/>
 <!-- <arg name="world_db_name" default="KV33_RTABMAP_DB_NICE1.db" unless="$(arg sim)"/> -->
-<arg name="world_db_name" default="rtabmap.db" unless="$(arg sim)"/>
+<!-- <arg name="world_db_name" default="rtabmap.db" unless="$(arg sim)"/> -->
 <!--  -->
 <arg name="database_path" default="$(find omnibot_platform_navigation)/maps/$(arg world_db_name)"/>
 <!-- <arg name="database_path" default="/home/ivan/.ros/rtabmap.db"/> -->
@@ -27,115 +27,126 @@
         <param name="database_path" type="string" value="$(find omnibot_platform_navigation)/maps/$(arg world_db_name)" unless="$(arg localization)"/><!-- Path of the RTAB-Map's database. -->
         <!-- <param name="database_path" type="string" value="~/.ros/rtabmap.db" unless="$(arg localization)"/>Path of the RTAB-Map's database. -->
         <param name="database_path" type="string" value="$(arg database_path)" if="$(arg localization)"/><!-- Path of the RTAB-Map's database. -->
-        <param name="use_action_for_goal" type="bool" value="true"/><!-- Planning Use actionlib to send the metric goals to move_base. The advantage over just connecting goal_out to move_base_simple/ goal is that rtabmap can have a feedback if the goal is reached or if move_base has failed. See move_base Action API for more info. -->
+      
         <!-- RTAB-Map's parameters -->
         <!-- ################################################### -->
-        <param name="Rtabmap/TimeThr" type="string" value="0" />
-        <param name="Rtabmap/DetectionRate" type="string" value="10" />
-        <param name="Rtabmap/ImageBufferSize" type="string" value="0" />
-        <param name="Rtabmap/StartNewMapOnLoopClosure" value="true"/> 
-        <param name="Rtabmap/PublishStats" type="string" value="true"/>
+        <param name="Rtabmap/TimeThr"                   type="string" value="700" />
+        <param name="Rtabmap/DetectionRate"             type="string" value="10" />
+        <param name="Rtabmap/ImageBufferSize"           type="string" value="0" />
+        <param name="Rtabmap/StartNewMapOnLoopClosure"  type="string" value="true"/> 
+        <param name="Rtabmap/PublishStats"              type="string" value="true"/>
+        <!-- ################################################### -->
+        <param name="Kp/MaxDepth"                       type="string" value="4.0" />
         <!-- ################################################### -->
-        <param name="Kp/MaxDepth" type="string" value="4.0" />
+        <param name="RGBD/AngularUpdate"                type="string" value="0.01" /><!-- Update map only if the robot is moving -->
+        <param name="RGBD/LinearUpdate"                 type="string" value="0.01" /><!-- Update map only if the robot is moving -->
+        <param name="RGBD/OptimizeMaxError"             type="double" value="0.1"/>
+        <param name="RGBD/ProximityPathMaxNeighbors"    type="string" value="0" />
+        <param name="RGBD/CreateOccupancyGrid"          type="string" value="true" /> <!-- The proximity detection using only laser scans is disabled-->
+        <param name="RGBD/StartAtOrigin"                type="string" value="false"/>
+        <param name="RGBD/NeighborLinkRefining"         type="string" value="true"/>  <!-- Do odometry correction with consecutive laser scans -->
+        <param name="RGBD/ProximityBySpace"             type="string" value="true" /> <!-- Local loop closure detection (using estimated position) with locations in WM -->
+        <param name="RGBD/ProximityByTime"              type="string" value="true"/> <!-- Local loop closure detection with locations in STM -->
+        <param name="RGBD/OptimizeFromGraphEnd"         type="string" value="false" /><!-- Set to false to generate map correction between /map and /odom -->
+        <param name="RGBD/LocalRadius"                  type="string" value="5"/>     <!-- limit length of proximity detections -->
         <!-- ################################################### -->
-        <param name="RGBD/AngularUpdate" type="string" value="0.01" /><!-- Update map only if the robot is moving -->
-        <param name="RGBD/LinearUpdate" type="string" value="0.01" /><!-- Update map only if the robot is moving -->
-        <param name="RGBD/OptimizeMaxError"     type="double" value="3"/>
-        <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="0" />
-        <param name="RGBD/CreateOccupancyGrid" type="string" value="true" />
-        <param name="RGBD/SavedLocalizationIgnored"  type="string" value="true"/>
-        <param name="RGBD/StartAtOrigin" value="false"/>
-        <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>  <!-- Do odometry correction with consecutive laser scans -->
-        <param name="RGBD/ProximityBySpace" type="string" value="true" /> <!-- Local loop closure detection (using estimated position) with locations in WM -->
-        <param name="RGBD/ProximityByTime"      type="string" value="true"/> <!-- Local loop closure detection with locations in STM -->
-        <param name="RGBD/OptimizeFromGraphEnd" type="string" value="true" /><!-- Set to false to generate map correction between /map and /odom -->
-        <param name="RGBD/LocalRadius"          type="string" value="5"/>     <!-- limit length of proximity detections -->
+        <param name="Grid/MaxGroundHeight"              type="string" value="0.05"/>
+        <param name="Grid/MaxObstacleHeight"            type="string" value="2.0"/>
+        <param name="Grid/CellSize"                     type="string" value="0.05" />
+        <param name="Grid/NormalsSegmentation"          type="string" value="true"/>
+        <param name="Grid/Sensor"                       type="string" value="2" />
+        <param name="Grid/RayTracing"                   type="string" value="true" />
+        <param name="Grid/3D"                           type="string" value="true"/>
         <!-- ################################################### -->
-        <param name="Grid/MaxGroundHeight" value="0.05"/>
-        <param name="Grid/MaxObstacleHeight" value="2.0"/>
-        <param name="Grid/CellSize" value="0.05" />
-        <param name="Grid/NormalsSegmentation" value="true"/>
-        <param name="Grid/Sensor" type="string" value="2" />
-        <param name="Grid/RayTracing" value="true" />
-        <param name="Grid/FromDepth"  type="string" value="true"/>
-        <param name="Grid/3D" value="true"/>
+        <param name="GridGlobal/MinSize" type="string"  value="10" />
         <!-- ################################################### -->
-        <param name="GridGlobal/MinSize" type="string" value="10" />
+        <param name="Reg/Force3DoF"                     type="string" value="true" />
+        <param name="Reg/Strategy"                      type="string" value="2" /><!-- Loop closure transformation: 0=Visual, 1=ICP, 2=Visual+ICP -->
         <!-- ################################################### -->
-        <param name="Reg/Force3DoF" type="string" value="true" />
-        <param name="Reg/Strategy" type="string" value="2" /><!-- Loop closure transformation: 0=Visual, 1=ICP, 2=Visual+ICP -->
+        <param name="Mem/RehearsalSimilarity"           type="string" value="0.30"/>
+        <param name="Mem/DepthAsMask"                   type="bool"   value="false"/>
+        <param name="Mem/IncrementalMemory"             type="string" value="false"     if="$(arg localization)"/>
+        <param name="Mem/IncrementalMemory"             type="string" value="true"     unless="$(arg localization)"/>
+        <param name="Mem/InitWMWithAllNodes"            type="string" value="$(arg localization)"/>
+        <param name="Mem/STMSize"                       type="string" value="30"/>    <!-- increased to 30 to avoid adding too many loop closures on just seen locations -->
         <!-- ################################################### -->
-        <param name="Mem/RehearsalSimilarity" type="string" value="0.30" />
-        <param name="Mem/IncrementalMemory"  type="string" value="false"     if="$(arg localization)"/>
-        <param name="Mem/IncrementalMemory"  type="string" value="true"     unless="$(arg localization)"/>
-        <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
-        <param name="Mem/STMSize"               type="string" value="30"/>    <!-- increased to 30 to avoid adding too many loop closures on just seen locations -->
+        <param name="Icp/PM" 			                type="bool"     value="true"/><!--actually, it is Trimmed ICP-->
+        <param name="Icp/PMMatcherKnn"                  type="int"      value="1"/>
+        <param name="Icp/PMConfig "                     type="string"   value=""/>
+        <param name="Icp/PMOutlierRatio"                type="string"   value="0.6"/><!--default 0.95-->
+        <param name="Icp/CorrespondenceRatio"           type="string"   value="0.6"/>      <!--when libpointmatcher was found, the default is pm instead of icp algorithm in PCL. But the following parameters still work, they work before parameters of PM -->
+        <param name="Icp/MaxCorrespondenceDistance"     type="string"   value="10"/><!--if pointmatcherlib wasn't found, the default is 0.05, else default 0.1-->
+        <param name="Icp/MaxTranslation"                type="string"   value="1"/>
+        <param name="Icp/MaxRotation"                   type="string"   value="0.78"/>
+        <param name="Icp/Iterations"                    type="int"      value="50"/>
+        <param name="Icp/PointToPlaneMinComplexity"     type="string"   value="0.95"/>
+        
         <!-- ################################################### -->
-        <param name="Icp/CorrespondenceRatio"   type="string" value="0.25"/>   <!-- minimum scan overlap to accept loop closure -->
-        <param name="Icp/PointToPlane"          type="string" value="true"/>
-        <param name="Icp/MaxCorrespondenceDistance"  type="string" value="0.15"/>
-        <param name="Icp/VoxelSize"             type="string" value="0.05"/>
+        <param name="Vis/MinInliers"                    type="string" value="15" /><!-- 3D visual words minimum inliers to accept loop closure -->
+        <param name="Vis/InlierDistance"                type="string" value="0.1" /> <!-- 3D visual words correspondence distance -->
         <!-- ################################################### -->
-        <param name="Vis/MinInliers" type="string" value="15" /><!-- 3D visual words minimum inliers to accept loop closure -->
-        <param name="Vis/InlierDistance" type="string" value="0.1" /> <!-- 3D visual words correspondence distance -->
-
+        <param name="Optimizer/Slam2D"                  type="string" value="true"/>
+        <!-- ################################################### -->
+        <param name="Odom/Strategy"                     type="string"  value="0"/>
+        <param name="Odom/Holonomic"		            type="string"  value="true"/>
+        <param name="Odom/FilteringStrategy"            type="string"  value="0"/>
 <!-- ################################################################################################################################################################# -->
         <!-- REMAPS -->
-        <remap from="odom" to="odom"/>
-        <remap from="scan" to="/$(arg camera2)/scan" />
-        <remap from="rgb/image" to="/$(arg camera2)/color/image_raw" />
-        <remap from="depth/image" to="/$(arg camera2)/aligned_depth_to_color/image_raw" />
-        <remap from="rgb/camera_info" to="/$(arg camera2)/color/camera_info" />
-        <!-- <remap from="rgbd_image" to="/$(arg camera2)/aligned_depth_to_color/image_raw"/> -->
+        <remap from="odom"                              to="odom"/>
+        <remap from="scan"                              to="/$(arg camera2)/scan" />
+        <remap from="rgb/image"                         to="/$(arg camera2)/color/image_raw" />
+        <remap from="depth/image"                       to="/$(arg camera2)/aligned_depth_to_color/image_raw" />
+        <remap from="rgb/camera_info"                   to="/$(arg camera2)/color/camera_info" />
+        <!-- <remap from="rgbd_image"                   to="/$(arg camera2)/aligned_depth_to_color/image_raw"/> -->
         <remap from="grid_map" to="/map"/>  <!-- gridmap => map-->
         <!-- SUBSCRIPTIONS -->
-        <param name="subscribe_depth" type="bool" value="true" /> <!-- Subscribe to depth image -->
-        <param name="subscribe_scan" type="bool" value="true" /><!-- Subscribe to laser scan -->
-        <param name="subscribe_scan_cloud" type="bool" value="false" /><!-- Subscribe to laser scan pointcloud -->
-        <param name="subscribe_stereo" type="bool" value="false" /><!-- Subscribe to stereo images -->
-        <param name="subscribe_rgbd" type="bool" value="false" /><!-- Subsribe to rgbd_image topic. -->
-        <!-- TF -->
-        <param name="frame_id" type="string" value="base_footprint" /><!-- The frame attached to the mobile base. -->
-        <param name="map_frame_id" type="string" value="map" /><!-- The frame attached to the map. -->
-        <param name="odom_frame_id" type="string" value="odom" /><!-- The frame attached to odometry. If empty,rtabmap will subscribe to odom topic to get odometry. If set, odometry is got from tf (in this case,the covariance value is fixed by odom_tf_angular_variance and odom_tf_linear_variance). -->
-        <param name="odom_tf_linear_variance" type="double" value="0.001"/><!-- When odom_frame_id is used, the first 3 values of the diagonal of the 6x6 covariance matrix are set to this value. -->
-        <param name="odom_tf_angular_variance" type="double" value="0.001"/> <!-- When odom_frame_id is used, the last 3 values of the diagonal of the 6x6 covariance matrix are set to this value. -->
-        <param name="queue_size" type="int" value="10"/><!-- Size of message queue for each synchronized topic. -->
-        <param name="publish_tf" type="bool" value="true"/>    <!-- Publish TF from / map to / odom. -->
-        <param name="tf_delay" type="double" value="0.05"/> <!-- Rate at which the TF from /map to /odom is published (20 Hz). -->
-        <param name="wait_for_transform" type="bool" value="true"/><!-- Wait (maximum wait_for_transform_duration sec) for transform when a tf transform is not still available. -->
-        <param name="wait_for_transform_duration" type="double" value="5.0"/><!-- Wait duration for wait_for_transform. -->
+        <param name="subscribe_depth"                   type="bool"     value="true" /> <!-- Subscribe to depth image -->
+        <param name="subscribe_scan"                    type="bool"     value="true" /><!-- Subscribe to laser scan -->
+        <param name="subscribe_scan_cloud"              type="bool"     value="false" /><!-- Subscribe to laser scan pointcloud -->
+        <param name="subscribe_stereo"                  type="bool"     value="false" /><!-- Subscribe to stereo images -->
+        <param name="subscribe_rgbd"                    type="bool"     value="false" /><!-- Subsribe to rgbd_image topic. -->
+        <!-- TF --> 
+        <param name="frame_id"                          type="string"   value="base_footprint" /><!-- The frame attached to the mobile base. -->
+        <param name="map_frame_id"                      type="string"   value="map" /><!-- The frame attached to the map. -->
+        <param name="odom_frame_id"                     type="string"   value="odom" /><!-- The frame attached to odometry. If empty,rtabmap will subscribe to odom topic to get odometry. If set, odometry is got from tf (in this case,the covariance value is fixed by odom_tf_angular_variance and odom_tf_linear_variance). -->
+        <param name="odom_tf_linear_variance"           type="double"   value="0.001"/><!-- When odom_frame_id is used, the first 3 values of the diagonal of the 6x6 covariance matrix are set to this value. -->
+        <param name="odom_tf_angular_variance"          type="double"   value="0.001"/> <!-- When odom_frame_id is used, the last 3 values of the diagonal of the 6x6 covariance matrix are set to this value. -->
+        <param name="queue_size"                        type="int"      value="10"/><!-- Size of message queue for each synchronized topic. -->
+        <param name="publish_tf"                        type="bool"     value="true"/>    <!-- Publish TF from / map to / odom. -->
+        <param name="tf_delay"                          type="double"   value="0.05"/> <!-- Rate at which the TF from /map to /odom is published (20 Hz). -->
+        <param name="wait_for_transform"                type="bool"     value="true"/><!-- Wait (maximum wait_for_transform_duration sec) for transform when a tf transform is not still available. -->
+        <param name="wait_for_transform_duration"       type="double"   value="5.0"/><!-- Wait duration for wait_for_transform. -->
         <!-- GENERATORS -->
-        <param name="gen_scan" type="bool" value="false"/> <!-- Generate laser scans from depth images (using the middle horizontal line of the depth image). Not generated if subscribe_scan or subscribe_scan_cloud are true. -->
-        <param name="gen_scan_max_depth" type="double" value="4.0"/><!-- Maximum depth of the laser scans generated. -->
-        <param name="gen_depth" type="bool" value="false"/><!-- Generate depth image from scan_cloud projection into RGB camera, taking into account displacement of the RGB camera accordingly to odometry and lidar frames. It works like pointcloud_to_depthimage node, but at slam frequency. -->
-        <param name="gen_depth_decimation" type="int" value="1"/><!-- Scale down image size of the camera info received (creating smaller depth image). -->
-        <param name="gen_depth_fill_holes_size" type="int" value="0"/><!-- Fill holes of empty pixels up to this size. Values are interpolated from neighbor depth values. 0 means disabled. -->
-        <param name="gen_depth_fill_iterations" type="double" value="0.1"/><!-- Maximum depth error (m) to interpolate. -->
-        <param name="gen_depth_fill_holes_error" type="int" value="1"/><!-- Number of iterations to fill holes. -->
+        <param name="gen_scan"                          type="bool" value="false"/> <!-- Generate laser scans from depth images (using the middle horizontal line of the depth image). Not generated if subscribe_scan or subscribe_scan_cloud are true. -->
+        <param name="gen_scan_max_depth"                type="double" value="4.0"/><!-- Maximum depth of the laser scans generated. -->
+        <param name="gen_depth"                         type="bool" value="false"/><!-- Generate depth image from scan_cloud projection into RGB camera, taking into account displacement of the RGB camera accordingly to odometry and lidar frames. It works like pointcloud_to_depthimage node, but at slam frequency. -->
+        <param name="gen_depth_decimation"              type="int" value="1"/><!-- Scale down image size of the camera info received (creating smaller depth image). -->
+        <param name="gen_depth_fill_holes_size"         type="int" value="0"/><!-- Fill holes of empty pixels up to this size. Values are interpolated from neighbor depth values. 0 means disabled. -->
+        <param name="gen_depth_fill_iterations"         type="double" value="0.1"/><!-- Maximum depth error (m) to interpolate. -->
+        <param name="gen_depth_fill_holes_error"        type="int" value="1"/><!-- Number of iterations to fill holes. -->
         <!-- SYNC         -->
-        <param name="odom_sensor_sync" type="bool" value="true"/><!-- Adjust image and scan poses relatively to odometry pose for each node added to graph. For example, if an image received at t=1s has been synchronized with a scan at t=1.1s (and our reference stamp is the scan) and the robot is moving forward at 1 m/s, then we will ask TF to know the motion between t=1s and t=1.1s (which would be 0.1 m) for the camera to adjust its local transform (-10 cm) relative to scan frame at scan stamp. This also applies to multi-camera synchronization. -->
-        <param name="approx_sync" type="bool" value="true"/> <!-- Use approximate time synchronization of input messages. If false, note that the odometry input must have also exactly the same timestamps than the input images. -->
+        <param name="odom_sensor_sync"                  type="bool" value="true"/><!-- Adjust image and scan poses relatively to odometry pose for each node added to graph. For example, if an image received at t=1s has been synchronized with a scan at t=1.1s (and our reference stamp is the scan) and the robot is moving forward at 1 m/s, then we will ask TF to know the motion between t=1s and t=1.1s (which would be 0.1 m) for the camera to adjust its local transform (-10 cm) relative to scan frame at scan stamp. This also applies to multi-camera synchronization. -->
+        <param name="approx_sync"                       type="bool" value="true"/> <!-- Use approximate time synchronization of input messages. If false, note that the odometry input must have also exactly the same timestamps than the input images. -->
         <!-- MAPPING -->
-        <param name="map_filter_radius" type="double" value="0.0"/><!-- Mapping Filter nodes before creating the maps. Only load data for one node in the filter radius (the latest data is used) up to filter angle (map_filter_angle). Set to 0.0 to disable node filtering. Used for all published maps. -->
-        <param name="map_filter_angle" type="double" value="30.0"/><!-- Mapping Angle used when filtering nodes before creating the maps. See also map_filter_radius. Used for all published maps. -->
-        <param name="map_cleanup" type="bool" value="true"/><!-- Mapping If there is no subscription to any map cloud_map, grid_map or proj_map, clear the corresponding data. -->
-        <param name="latch" type="bool" value="true"/><!-- Mapping If true, the last message published on the map topics will be saved and sent to new subscribers when they connect. -->
-        <param name="map_always_update" type="bool" value="false"/><!-- Mapping Always update the occupancy grid map even if the graph has not been updated (e.g., by default when the robot is not moving, the graph is not growing). For example, if the environment is dynamic, we may want to update the map even when to robot is not moving. Another approach would be to force rtabmap to always update the graph (by setting RGBD/LinearUpdate to 0), which can be not efficient over time as the map will grow even if the robot doesn't move. -->
-        <param name="map_empty_ray_tracing" type="bool" value="true"/><!-- Mapping Do ray tracing to fill unknown space for invalid 2D scan's rays (assuming invalid rays to be infinite). Used only when map_always_update is also true. -->
-        <param name="cloud_output_voxelized" type="bool" value="true"/><!-- Mapping Do a final voxel filtering after all clouds are assembled with a voxel size equals to Grid/CellSize. Used for cloud_map published topic. -->
-        <param name="cloud_subtract_filtering" type="bool" value="true"/><!-- Mapping When appending a new cloud to cloud_map, filter points that have less neighbors than cloud_subtract_filtering_min_neighbors in the radius Grid/CellSize. This will helps to reconstruct the whole map' s cloud faster when a loop closure happens. Used for cloud_map published topic. -->
-        <param name="cloud_subtract_filtering_min_neighbors" type="int" value="2"/><!-- Mapping See cloud_subtract_filtering parameter's description. -->
+        <param name="map_filter_radius"                         type="double" value="0.0"/><!-- Mapping Filter nodes before creating the maps. Only load data for one node in the filter radius (the latest data is used) up to filter angle (map_filter_angle). Set to 0.0 to disable node filtering. Used for all published maps. -->
+        <param name="map_filter_angle"                          type="double" value="30.0"/><!-- Mapping Angle used when filtering nodes before creating the maps. See also map_filter_radius. Used for all published maps. -->
+        <param name="map_cleanup"                               type="bool" value="true"/><!-- Mapping If there is no subscription to any map cloud_map, grid_map or proj_map, clear the corresponding data. -->
+        <param name="latch"                                     type="bool" value="true"/><!-- Mapping If true, the last message published on the map topics will be saved and sent to new subscribers when they connect. -->
+        <param name="map_always_update"                         type="bool" value="false"/><!-- Mapping Always update the occupancy grid map even if the graph has not been updated (e.g., by default when the robot is not moving, the graph is not growing). For example, if the environment is dynamic, we may want to update the map even when to robot is not moving. Another approach would be to force rtabmap to always update the graph (by setting RGBD/LinearUpdate to 0), which can be not efficient over time as the map will grow even if the robot doesn't move. -->
+        <param name="map_empty_ray_tracing"                     type="bool" value="true"/><!-- Mapping Do ray tracing to fill unknown space for invalid 2D scan's rays (assuming invalid rays to be infinite). Used only when map_always_update is also true. -->
+        <param name="cloud_output_voxelized"                    type="bool" value="true"/><!-- Mapping Do a final voxel filtering after all clouds are assembled with a voxel size equals to Grid/CellSize. Used for cloud_map published topic. -->
+        <param name="cloud_subtract_filtering"                  type="bool" value="true"/><!-- Mapping When appending a new cloud to cloud_map, filter points that have less neighbors than cloud_subtract_filtering_min_neighbors in the radius Grid/CellSize. This will helps to reconstruct the whole map' s cloud faster when a loop closure happens. Used for cloud_map published topic. -->
+        <param name="cloud_subtract_filtering_min_neighbors"    type="int" value="2"/><!-- Mapping See cloud_subtract_filtering parameter's description. -->
         <!-- RGBD -->
         <param name="rgbd_cameras" type="int" value="1"/><!-- Number of RGB-D cameras to use (when subscribe_rgbd is true). Well for now, a maximum of 4 cameras can be synchronized at the same time. If > 1, the rgbd_image topics should contain the camera index starting with 0. For example, if we have 2 cameras, you should set rgbd_image0 and rgbd_image1 topics. -->
-        <!-- LOCALIZATION MODE -->
-        
 
+        
+        <param name="use_action_for_goal" type="bool" value="true"/><!-- Planning Use actionlib to send the metric goals to move_base. The advantage over just connecting goal_out to move_base_simple/ goal is that rtabmap can have a feedback if the goal is reached or if move_base has failed. See move_base Action API for more info. -->
     </node>
 </group>
 <node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan">
     <param name="scan_height" value="0.07" />
-    <param name="output_frame_id" value="d400_link" />
+    <param name="output_frame_id" value="$(arg camera2)_link" />
     <param name="range_min" type="double" value="0.1" />
     <param name="range_max" type="double" value="4.0" />
     <remap from="image" to="/$(arg camera2)/aligned_depth_to_color/image_raw" />

From 66a6e05ccbfe395167f24dcd6db0cbdf60998c03 Mon Sep 17 00:00:00 2001
From: IvanSnap360 <ivpetr2003@gmail.com>
Date: Sun, 4 Jun 2023 09:11:10 +0300
Subject: [PATCH 03/13] some changes in rtabmap launch file

---
 src/omnibot_bringup/rviz/rviz.rviz            | 1223 ++++++++++++++++-
 .../launch/rtabmap.launch                     |  186 ++-
 2 files changed, 1267 insertions(+), 142 deletions(-)

diff --git a/src/omnibot_bringup/rviz/rviz.rviz b/src/omnibot_bringup/rviz/rviz.rviz
index 8d9f466..3a41888 100644
--- a/src/omnibot_bringup/rviz/rviz.rviz
+++ b/src/omnibot_bringup/rviz/rviz.rviz
@@ -12,7 +12,7 @@ Panels:
         - /move_base1/RRT_Global_Planner1/global_plan1
         - /move_base1/EbandPlanner1
       Splitter Ratio: 0.5080732107162476
-    Tree Height: 715
+    Tree Height: 1656
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -65,14 +65,24 @@ Visualization Manager:
         All Enabled: true
         ang_link0:
           Value: true
+        ang_link1:
+          Value: true
+        ang_link2:
+          Value: true
+        ang_link3:
+          Value: true
+        ang_link4:
+          Value: true
         ang_link5:
           Value: true
+        ang_link6:
+          Value: true
+        ang_link7:
+          Value: true
         base_footprint:
           Value: true
         base_link:
           Value: true
-        d400_aligned_depth_to_color_frame:
-          Value: true
         d400_bottom_screw_frame:
           Value: true
         d400_color_frame:
@@ -83,14 +93,6 @@ Visualization Manager:
           Value: true
         d400_depth_optical_frame:
           Value: true
-        d400_infra1_frame:
-          Value: true
-        d400_infra1_optical_frame:
-          Value: true
-        d400_infra2_frame:
-          Value: true
-        d400_infra2_optical_frame:
-          Value: true
         d400_left_ir_frame:
           Value: true
         d400_left_ir_optical_frame:
@@ -105,10 +107,66 @@ Visualization Manager:
           Value: true
         left_back_mecanum_wheel_link:
           Value: true
+        left_back_roller_1_1_link:
+          Value: true
+        left_back_roller_1_2_link:
+          Value: true
+        left_back_roller_1_3_link:
+          Value: true
+        left_back_roller_1_4_link:
+          Value: true
+        left_back_roller_1_5_link:
+          Value: true
+        left_back_roller_1_6_link:
+          Value: true
+        left_back_roller_1_7_link:
+          Value: true
+        left_back_roller_2_1_link:
+          Value: true
+        left_back_roller_2_2_link:
+          Value: true
+        left_back_roller_2_3_link:
+          Value: true
+        left_back_roller_2_4_link:
+          Value: true
+        left_back_roller_2_5_link:
+          Value: true
+        left_back_roller_2_6_link:
+          Value: true
+        left_back_roller_2_7_link:
+          Value: true
         left_front_mecanum_adapter_link:
           Value: true
         left_front_mecanum_wheel_link:
           Value: true
+        left_front_roller_1_1_link:
+          Value: true
+        left_front_roller_1_2_link:
+          Value: true
+        left_front_roller_1_3_link:
+          Value: true
+        left_front_roller_1_4_link:
+          Value: true
+        left_front_roller_1_5_link:
+          Value: true
+        left_front_roller_1_6_link:
+          Value: true
+        left_front_roller_1_7_link:
+          Value: true
+        left_front_roller_2_1_link:
+          Value: true
+        left_front_roller_2_2_link:
+          Value: true
+        left_front_roller_2_3_link:
+          Value: true
+        left_front_roller_2_4_link:
+          Value: true
+        left_front_roller_2_5_link:
+          Value: true
+        left_front_roller_2_6_link:
+          Value: true
+        left_front_roller_2_7_link:
+          Value: true
         map:
           Value: true
         odom:
@@ -117,10 +175,66 @@ Visualization Manager:
           Value: true
         right_back_mecanum_wheel_link:
           Value: true
+        right_back_roller_1_1_link:
+          Value: true
+        right_back_roller_1_2_link:
+          Value: true
+        right_back_roller_1_3_link:
+          Value: true
+        right_back_roller_1_4_link:
+          Value: true
+        right_back_roller_1_5_link:
+          Value: true
+        right_back_roller_1_6_link:
+          Value: true
+        right_back_roller_1_7_link:
+          Value: true
+        right_back_roller_2_1_link:
+          Value: true
+        right_back_roller_2_2_link:
+          Value: true
+        right_back_roller_2_3_link:
+          Value: true
+        right_back_roller_2_4_link:
+          Value: true
+        right_back_roller_2_5_link:
+          Value: true
+        right_back_roller_2_6_link:
+          Value: true
+        right_back_roller_2_7_link:
+          Value: true
         right_front_mecanum_adapter_link:
           Value: true
         right_front_mecanum_wheel_link:
           Value: true
+        right_front_roller_1_1_link:
+          Value: true
+        right_front_roller_1_2_link:
+          Value: true
+        right_front_roller_1_3_link:
+          Value: true
+        right_front_roller_1_4_link:
+          Value: true
+        right_front_roller_1_5_link:
+          Value: true
+        right_front_roller_1_6_link:
+          Value: true
+        right_front_roller_1_7_link:
+          Value: true
+        right_front_roller_2_1_link:
+          Value: true
+        right_front_roller_2_2_link:
+          Value: true
+        right_front_roller_2_3_link:
+          Value: true
+        right_front_roller_2_4_link:
+          Value: true
+        right_front_roller_2_5_link:
+          Value: true
+        right_front_roller_2_6_link:
+          Value: true
+        right_front_roller_2_7_link:
+          Value: true
         t265_accel_frame:
           Value: true
         t265_accel_optical_frame:
@@ -150,25 +264,24 @@ Visualization Manager:
       Show Axes: true
       Show Names: true
       Tree:
-        ang_link5:
-          tip_link:
-            {}
-        left_back_mecanum_wheel_link:
-          left_back_mecanum_adapter_link:
-            {}
-        left_front_mecanum_wheel_link:
-          left_front_mecanum_adapter_link:
-            {}
         map:
           odom:
             base_footprint:
               ang_link0:
-                {}
+                ang_link1:
+                  ang_link2:
+                    ang_link3:
+                      ang_link4:
+                        ang_link5:
+                          ang_link6:
+                            {}
+                          ang_link7:
+                            {}
+                          tip_link:
+                            {}
               base_link:
                 d400_bottom_screw_frame:
                   d400_link:
-                    d400_aligned_depth_to_color_frame:
-                      {}
                     d400_depth_frame:
                       d400_color_frame:
                         d400_color_optical_frame:
@@ -181,12 +294,130 @@ Visualization Manager:
                       d400_right_ir_frame:
                         d400_right_ir_optical_frame:
                           {}
-                    d400_infra1_frame:
-                      d400_infra1_optical_frame:
-                        {}
-                    d400_infra2_frame:
-                      d400_infra2_optical_frame:
-                        {}
+                left_back_mecanum_wheel_link:
+                  left_back_mecanum_adapter_link:
+                    {}
+                  left_back_roller_1_1_link:
+                    {}
+                  left_back_roller_1_2_link:
+                    {}
+                  left_back_roller_1_3_link:
+                    {}
+                  left_back_roller_1_4_link:
+                    {}
+                  left_back_roller_1_5_link:
+                    {}
+                  left_back_roller_1_6_link:
+                    {}
+                  left_back_roller_1_7_link:
+                    {}
+                  left_back_roller_2_1_link:
+                    {}
+                  left_back_roller_2_2_link:
+                    {}
+                  left_back_roller_2_3_link:
+                    {}
+                  left_back_roller_2_4_link:
+                    {}
+                  left_back_roller_2_5_link:
+                    {}
+                  left_back_roller_2_6_link:
+                    {}
+                  left_back_roller_2_7_link:
+                    {}
+                left_front_mecanum_wheel_link:
+                  left_front_mecanum_adapter_link:
+                    {}
+                  left_front_roller_1_1_link:
+                    {}
+                  left_front_roller_1_2_link:
+                    {}
+                  left_front_roller_1_3_link:
+                    {}
+                  left_front_roller_1_4_link:
+                    {}
+                  left_front_roller_1_5_link:
+                    {}
+                  left_front_roller_1_6_link:
+                    {}
+                  left_front_roller_1_7_link:
+                    {}
+                  left_front_roller_2_1_link:
+                    {}
+                  left_front_roller_2_2_link:
+                    {}
+                  left_front_roller_2_3_link:
+                    {}
+                  left_front_roller_2_4_link:
+                    {}
+                  left_front_roller_2_5_link:
+                    {}
+                  left_front_roller_2_6_link:
+                    {}
+                  left_front_roller_2_7_link:
+                    {}
+                right_back_mecanum_wheel_link:
+                  right_back_mecanum_adapter_link:
+                    {}
+                  right_back_roller_1_1_link:
+                    {}
+                  right_back_roller_1_2_link:
+                    {}
+                  right_back_roller_1_3_link:
+                    {}
+                  right_back_roller_1_4_link:
+                    {}
+                  right_back_roller_1_5_link:
+                    {}
+                  right_back_roller_1_6_link:
+                    {}
+                  right_back_roller_1_7_link:
+                    {}
+                  right_back_roller_2_1_link:
+                    {}
+                  right_back_roller_2_2_link:
+                    {}
+                  right_back_roller_2_3_link:
+                    {}
+                  right_back_roller_2_4_link:
+                    {}
+                  right_back_roller_2_5_link:
+                    {}
+                  right_back_roller_2_6_link:
+                    {}
+                  right_back_roller_2_7_link:
+                    {}
+                right_front_mecanum_wheel_link:
+                  right_front_mecanum_adapter_link:
+                    {}
+                  right_front_roller_1_1_link:
+                    {}
+                  right_front_roller_1_2_link:
+                    {}
+                  right_front_roller_1_3_link:
+                    {}
+                  right_front_roller_1_4_link:
+                    {}
+                  right_front_roller_1_5_link:
+                    {}
+                  right_front_roller_1_6_link:
+                    {}
+                  right_front_roller_1_7_link:
+                    {}
+                  right_front_roller_2_1_link:
+                    {}
+                  right_front_roller_2_2_link:
+                    {}
+                  right_front_roller_2_3_link:
+                    {}
+                  right_front_roller_2_4_link:
+                    {}
+                  right_front_roller_2_5_link:
+                    {}
+                  right_front_roller_2_6_link:
+                    {}
+                  right_front_roller_2_7_link:
+                    {}
                 t265_pose_frame:
                   t265_link:
                     t265_accel_frame:
@@ -201,12 +432,6 @@ Visualization Manager:
                     t265_gyro_frame:
                       t265_gyro_optical_frame:
                         {}
-        right_back_mecanum_wheel_link:
-          right_back_mecanum_adapter_link:
-            {}
-        right_front_mecanum_wheel_link:
-          right_front_mecanum_adapter_link:
-            {}
       Update Interval: 0
       Value: true
     - Alpha: 1
@@ -221,7 +446,7 @@ Visualization Manager:
       Color: 255; 255; 255
       Color Transformer: RGB8
       Decay Time: 0
-      Enabled: false
+      Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Min Color: 0; 0; 0
@@ -232,11 +457,11 @@ Visualization Manager:
       Size (Pixels): 3
       Size (m): 0.03999999910593033
       Style: Boxes
-      Topic: /rtabmap/local_grid_obstacle
+      Topic: /rtabmap/cloud_map
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
-      Value: false
+      Value: true
     - Alpha: 0.699999988079071
       Class: rviz/Map
       Color Scheme: map
@@ -1285,7 +1510,7 @@ Visualization Manager:
       Name: move_base
     - Acceleration_Scaling_Factor: 0.1
       Class: moveit_rviz_plugin/MotionPlanning
-      Enabled: true
+      Enabled: false
       Move Group Namespace: ""
       MoveIt_Allow_Approximate_IK: false
       MoveIt_Allow_External_Program: true
@@ -1314,6 +1539,461 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
+          ang_link0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          base_footprint:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          d400_bottom_screw_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_color_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_color_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_depth_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_depth_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_left_ir_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_left_ir_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          d400_right_ir_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_right_ir_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          left_back_mecanum_adapter_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_mecanum_wheel_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_mecanum_adapter_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_mecanum_wheel_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_mecanum_adapter_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_mecanum_wheel_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_mecanum_adapter_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_mecanum_wheel_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          t265_accel_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_accel_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_fisheye1_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_fisheye1_rgb_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_fisheye2_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_fisheye2_rgb_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_gyro_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_gyro_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_pose_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          tip_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
         Loop Animation: false
         Robot Alpha: 0.5
         Robot Color: 150; 50; 150
@@ -1360,10 +2040,465 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
+          ang_link0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          base_footprint:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          d400_bottom_screw_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_color_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_color_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_depth_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_depth_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_left_ir_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_left_ir_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          d400_right_ir_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_right_ir_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          left_back_mecanum_adapter_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_mecanum_wheel_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_mecanum_adapter_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_mecanum_wheel_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_mecanum_adapter_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_mecanum_wheel_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_mecanum_adapter_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_mecanum_wheel_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          t265_accel_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_accel_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_fisheye1_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_fisheye1_rgb_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_fisheye2_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_fisheye2_rgb_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_gyro_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_gyro_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_pose_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          tip_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
         Robot Alpha: 1
         Show Robot Collision: false
         Show Robot Visual: false
-      Value: true
+      Value: false
       Velocity_Scaling_Factor: 0.1
   Enabled: true
   Global Options:
@@ -1416,7 +2551,7 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 2002
+  Height: 2086
   Hide Left Dock: false
   Hide Right Dock: false
   Image:
@@ -1425,7 +2560,7 @@ Window Geometry:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
@@ -1434,6 +2569,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 3264
+  Width: 3696
   X: 144
-  Y: 54
+  Y: 0
diff --git a/src/omnibot_platform/omnibot_platform_navigation/launch/rtabmap.launch b/src/omnibot_platform/omnibot_platform_navigation/launch/rtabmap.launch
index 6fc1caa..85e0cbe 100644
--- a/src/omnibot_platform/omnibot_platform_navigation/launch/rtabmap.launch
+++ b/src/omnibot_platform/omnibot_platform_navigation/launch/rtabmap.launch
@@ -30,127 +30,117 @@
       
         <!-- RTAB-Map's parameters -->
         <!-- ################################################### -->
-        <param name="Rtabmap/TimeThr"                   type="string" value="700" />
-        <param name="Rtabmap/DetectionRate"             type="string" value="10" />
-        <param name="Rtabmap/ImageBufferSize"           type="string" value="0" />
-        <param name="Rtabmap/StartNewMapOnLoopClosure"  type="string" value="true"/> 
-        <param name="Rtabmap/PublishStats"              type="string" value="true"/>
+        <param name="Rtabmap/TimeThr"                           type="string"   value="700" />
+        <param name="Rtabmap/DetectionRate"                     type="string"   value="10" />
+        <param name="Rtabmap/ImageBufferSize"                   type="string"   value="0" />
+        <param name="Rtabmap/StartNewMapOnLoopClosure"          type="string"   value="true"/> 
+        <param name="Rtabmap/PublishStats"                      type="string"   value="true"/>
         <!-- ################################################### -->
-        <param name="Kp/MaxDepth"                       type="string" value="4.0" />
+        <param name="Kp/MaxDepth"                               type="string"   value="4.0" />
         <!-- ################################################### -->
-        <param name="RGBD/AngularUpdate"                type="string" value="0.01" /><!-- Update map only if the robot is moving -->
-        <param name="RGBD/LinearUpdate"                 type="string" value="0.01" /><!-- Update map only if the robot is moving -->
-        <param name="RGBD/OptimizeMaxError"             type="double" value="0.1"/>
-        <param name="RGBD/ProximityPathMaxNeighbors"    type="string" value="0" />
-        <param name="RGBD/CreateOccupancyGrid"          type="string" value="true" /> <!-- The proximity detection using only laser scans is disabled-->
-        <param name="RGBD/StartAtOrigin"                type="string" value="false"/>
-        <param name="RGBD/NeighborLinkRefining"         type="string" value="true"/>  <!-- Do odometry correction with consecutive laser scans -->
-        <param name="RGBD/ProximityBySpace"             type="string" value="true" /> <!-- Local loop closure detection (using estimated position) with locations in WM -->
-        <param name="RGBD/ProximityByTime"              type="string" value="true"/> <!-- Local loop closure detection with locations in STM -->
-        <param name="RGBD/OptimizeFromGraphEnd"         type="string" value="false" /><!-- Set to false to generate map correction between /map and /odom -->
-        <param name="RGBD/LocalRadius"                  type="string" value="5"/>     <!-- limit length of proximity detections -->
+        <param name="RGBD/AngularUpdate"                        type="string"   value="0.1" /><!-- Update map only if the robot is moving -->
+        <param name="RGBD/LinearUpdate"                         type="string"   value="0.1" /><!-- Update map only if the robot is moving -->
+        <param name="RGBD/OptimizeMaxError"                     type="double"   value="0"/> <!-- Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled-->
+        <param name="RGBD/ProximityPathMaxNeighbors"            type="string"   value="0" />
+        <param name="RGBD/CreateOccupancyGrid"                  type="string"   value="true" /> <!-- The proximity detection using only laser scans is disabled-->
+        <param name="RGBD/StartAtOrigin"                        type="string"   value="false"/>
+        <param name="RGBD/NeighborLinkRefining"                 type="string"   value="true"/>  <!-- Do odometry correction with consecutive laser scans -->
+        <param name="RGBD/ProximityBySpace"                     type="string"   value="true" /> <!-- Local loop closure detection (using estimated position) with locations in WM -->
+        <param name="RGBD/ProximityByTime"                      type="string"   value="true"/> <!-- Local loop closure detection with locations in STM -->
+        <param name="RGBD/OptimizeFromGraphEnd"                 type="string"   value="false" /><!-- Set to false to generate map correction between /map and /odom -->
+        <param name="RGBD/LocalRadius"                          type="string"   value="5"/>     <!-- limit length of proximity detections -->
         <!-- ################################################### -->
-        <param name="Grid/MaxGroundHeight"              type="string" value="0.05"/>
-        <param name="Grid/MaxObstacleHeight"            type="string" value="2.0"/>
-        <param name="Grid/CellSize"                     type="string" value="0.05" />
-        <param name="Grid/NormalsSegmentation"          type="string" value="true"/>
-        <param name="Grid/Sensor"                       type="string" value="2" />
-        <param name="Grid/RayTracing"                   type="string" value="true" />
-        <param name="Grid/3D"                           type="string" value="true"/>
+        <param name="Grid/MaxGroundHeight"                      type="string"   value="0.05"/>
+        <param name="Grid/MaxObstacleHeight"                    type="string"   value="2.0"/>
+        <param name="Grid/CellSize"                             type="string"   value="0.05" />
+        <param name="Grid/NormalsSegmentation"                  type="string"   value="true"/>
+        <param name="Grid/Sensor"                               type="string"   value="2" />
+        <param name="Grid/RayTracing"                           type="string"   value="true" />
+        <param name="Grid/3D"                                   type="string"   value="true"/>
         <!-- ################################################### -->
-        <param name="GridGlobal/MinSize" type="string"  value="10" />
+        <param name="GridGlobal/MinSize"                        type="string"   value="10" />
         <!-- ################################################### -->
-        <param name="Reg/Force3DoF"                     type="string" value="true" />
-        <param name="Reg/Strategy"                      type="string" value="2" /><!-- Loop closure transformation: 0=Visual, 1=ICP, 2=Visual+ICP -->
+        <param name="Reg/Force3DoF"                             type="string"   value="true" />
+        <param name="Reg/Strategy"                              type="string"   value="2" /><!-- Loop closure transformation: 0=Visual, 1=ICP, 2=Visual+ICP -->
         <!-- ################################################### -->
-        <param name="Mem/RehearsalSimilarity"           type="string" value="0.30"/>
-        <param name="Mem/DepthAsMask"                   type="bool"   value="false"/>
-        <param name="Mem/IncrementalMemory"             type="string" value="false"     if="$(arg localization)"/>
-        <param name="Mem/IncrementalMemory"             type="string" value="true"     unless="$(arg localization)"/>
-        <param name="Mem/InitWMWithAllNodes"            type="string" value="$(arg localization)"/>
-        <param name="Mem/STMSize"                       type="string" value="30"/>    <!-- increased to 30 to avoid adding too many loop closures on just seen locations -->
+        <param name="Mem/RehearsalSimilarity"                   type="string"   value="0.30"/>  <!--  Rehearsal similarity.-->
+        <param name="Mem/DepthAsMask"                           type="bool"     value="false"/> <!-- Use depth image as mask when extracting features for vocabulary-->
+        <param name="Mem/IncrementalMemory"                     type="string"   value="false"     if="$(arg localization)"/>
+        <param name="Mem/IncrementalMemory"                     type="string"   value="true"     unless="$(arg localization)"/>
+        <param name="Mem/InitWMWithAllNodes"                    type="string"   value="$(arg localization)"/>
+        <param name="Mem/STMSize"                               type="string"   value="30"/>    <!--  increased to 30 to avoid adding too many loop closures on just seen locations-->
         <!-- ################################################### -->
-        <param name="Icp/PM" 			                type="bool"     value="true"/><!--actually, it is Trimmed ICP-->
-        <param name="Icp/PMMatcherKnn"                  type="int"      value="1"/>
-        <param name="Icp/PMConfig "                     type="string"   value=""/>
-        <param name="Icp/PMOutlierRatio"                type="string"   value="0.6"/><!--default 0.95-->
-        <param name="Icp/CorrespondenceRatio"           type="string"   value="0.6"/>      <!--when libpointmatcher was found, the default is pm instead of icp algorithm in PCL. But the following parameters still work, they work before parameters of PM -->
-        <param name="Icp/MaxCorrespondenceDistance"     type="string"   value="10"/><!--if pointmatcherlib wasn't found, the default is 0.05, else default 0.1-->
-        <param name="Icp/MaxTranslation"                type="string"   value="1"/>
-        <param name="Icp/MaxRotation"                   type="string"   value="0.78"/>
-        <param name="Icp/Iterations"                    type="int"      value="50"/>
-        <param name="Icp/PointToPlaneMinComplexity"     type="string"   value="0.95"/>
-        
+        <param name="Icp/CorrespondenceRatio"      type="string" value="0.3"/>
         <!-- ################################################### -->
-        <param name="Vis/MinInliers"                    type="string" value="15" /><!-- 3D visual words minimum inliers to accept loop closure -->
-        <param name="Vis/InlierDistance"                type="string" value="0.1" /> <!-- 3D visual words correspondence distance -->
+        <param name="Vis/MinInliers"                            type="string"   value="15" /><!-- 3D visual words minimum inliers to accept loop closure -->
+        <param name="Vis/InlierDistance"                        type="string"   value="0.1" /> <!-- 3D visual words correspondence distance -->
         <!-- ################################################### -->
-        <param name="Optimizer/Slam2D"                  type="string" value="true"/>
+        <param name="Optimizer/Slam2D"                          type="string"   value="true"/>
         <!-- ################################################### -->
-        <param name="Odom/Strategy"                     type="string"  value="0"/>
-        <param name="Odom/Holonomic"		            type="string"  value="true"/>
-        <param name="Odom/FilteringStrategy"            type="string"  value="0"/>
+        <param name="Odom/Strategy"                             type="string"   value="5"/> <!-- 0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D-->
+        <param name="Odom/Holonomic"		                    type="string"   value="true"/> <!-- If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).-->
+        <param name="Odom/FilteringStrategy"                    type="string"   value="0"/> <!-- 0=No filtering 1=Kalman filtering 2=Particle filtering. This filter is used to smooth the odometry output.-->
 <!-- ################################################################################################################################################################# -->
         <!-- REMAPS -->
-        <remap from="odom"                              to="odom"/>
-        <remap from="scan"                              to="/$(arg camera2)/scan" />
-        <remap from="rgb/image"                         to="/$(arg camera2)/color/image_raw" />
-        <remap from="depth/image"                       to="/$(arg camera2)/aligned_depth_to_color/image_raw" />
-        <remap from="rgb/camera_info"                   to="/$(arg camera2)/color/camera_info" />
-        <!-- <remap from="rgbd_image"                   to="/$(arg camera2)/aligned_depth_to_color/image_raw"/> -->
-        <remap from="grid_map" to="/map"/>  <!-- gridmap => map-->
+        <remap from="odom"                                      to="odom"/>
+        <remap from="scan"                                      to="/$(arg camera2)/scan" />
+        <remap from="rgb/image"                                 to="/$(arg camera2)/color/image_raw" />
+        <remap from="depth/image"                               to="/$(arg camera2)/aligned_depth_to_color/image_raw" />
+        <remap from="rgb/camera_info"                           to="/$(arg camera2)/color/camera_info" />
+        <!-- <remap from="rgbd_image"                                to="/$(arg camera2)/aligned_depth_to_color/image_raw"/> -->
+        <remap from="grid_map"                                  to="/map"/>  <!-- gridmap => map-->
         <!-- SUBSCRIPTIONS -->
-        <param name="subscribe_depth"                   type="bool"     value="true" /> <!-- Subscribe to depth image -->
-        <param name="subscribe_scan"                    type="bool"     value="true" /><!-- Subscribe to laser scan -->
-        <param name="subscribe_scan_cloud"              type="bool"     value="false" /><!-- Subscribe to laser scan pointcloud -->
-        <param name="subscribe_stereo"                  type="bool"     value="false" /><!-- Subscribe to stereo images -->
-        <param name="subscribe_rgbd"                    type="bool"     value="false" /><!-- Subsribe to rgbd_image topic. -->
+        <param name="subscribe_depth"                           type="bool"     value="true" /> <!-- Subscribe to depth image -->
+        <param name="subscribe_scan"                            type="bool"     value="true" /><!-- Subscribe to laser scan -->
+        <param name="subscribe_scan_cloud"                      type="bool"     value="false" /><!-- Subscribe to laser scan pointcloud -->
+        <param name="subscribe_stereo"                          type="bool"     value="false" /><!-- Subscribe to stereo images -->
+        <param name="subscribe_rgbd"                            type="bool"     value="false" /><!-- Subsribe to rgbd_image topic. -->
         <!-- TF --> 
-        <param name="frame_id"                          type="string"   value="base_footprint" /><!-- The frame attached to the mobile base. -->
-        <param name="map_frame_id"                      type="string"   value="map" /><!-- The frame attached to the map. -->
-        <param name="odom_frame_id"                     type="string"   value="odom" /><!-- The frame attached to odometry. If empty,rtabmap will subscribe to odom topic to get odometry. If set, odometry is got from tf (in this case,the covariance value is fixed by odom_tf_angular_variance and odom_tf_linear_variance). -->
-        <param name="odom_tf_linear_variance"           type="double"   value="0.001"/><!-- When odom_frame_id is used, the first 3 values of the diagonal of the 6x6 covariance matrix are set to this value. -->
-        <param name="odom_tf_angular_variance"          type="double"   value="0.001"/> <!-- When odom_frame_id is used, the last 3 values of the diagonal of the 6x6 covariance matrix are set to this value. -->
-        <param name="queue_size"                        type="int"      value="10"/><!-- Size of message queue for each synchronized topic. -->
-        <param name="publish_tf"                        type="bool"     value="true"/>    <!-- Publish TF from / map to / odom. -->
-        <param name="tf_delay"                          type="double"   value="0.05"/> <!-- Rate at which the TF from /map to /odom is published (20 Hz). -->
-        <param name="wait_for_transform"                type="bool"     value="true"/><!-- Wait (maximum wait_for_transform_duration sec) for transform when a tf transform is not still available. -->
-        <param name="wait_for_transform_duration"       type="double"   value="5.0"/><!-- Wait duration for wait_for_transform. -->
+        <param name="frame_id"                                  type="string"   value="base_footprint" /><!-- The frame attached to the mobile base. -->
+        <param name="map_frame_id"                              type="string"   value="map" /><!-- The frame attached to the map. -->
+        <param name="odom_frame_id"                             type="string"   value="odom" /><!-- The frame attached to odometry. If empty,rtabmap will subscribe to odom topic to get odometry. If set, odometry is got from tf (in this case,the covariance value is fixed by odom_tf_angular_variance and odom_tf_linear_variance). -->
+        <param name="odom_tf_linear_variance"                   type="double"   value="0.001"/><!-- When odom_frame_id is used, the first 3 values of the diagonal of the 6x6 covariance matrix are set to this value. -->
+        <param name="odom_tf_angular_variance"                  type="double"   value="0.001"/> <!-- When odom_frame_id is used, the last 3 values of the diagonal of the 6x6 covariance matrix are set to this value. -->
+        <param name="queue_size"                                type="int"      value="10"/><!-- Size of message queue for each synchronized topic. -->
+        <param name="publish_tf"                                type="bool"     value="true"/>    <!-- Publish TF from / map to / odom. -->
+        <param name="tf_delay"                                  type="double"   value="0.05"/> <!-- Rate at which the TF from /map to /odom is published (20 Hz). -->
+        <param name="wait_for_transform"                        type="bool"     value="true"/><!-- Wait (maximum wait_for_transform_duration sec) for transform when a tf transform is not still available. -->
+        <param name="wait_for_transform_duration"               type="double"   value="5.0"/><!-- Wait duration for wait_for_transform. -->
         <!-- GENERATORS -->
-        <param name="gen_scan"                          type="bool" value="false"/> <!-- Generate laser scans from depth images (using the middle horizontal line of the depth image). Not generated if subscribe_scan or subscribe_scan_cloud are true. -->
-        <param name="gen_scan_max_depth"                type="double" value="4.0"/><!-- Maximum depth of the laser scans generated. -->
-        <param name="gen_depth"                         type="bool" value="false"/><!-- Generate depth image from scan_cloud projection into RGB camera, taking into account displacement of the RGB camera accordingly to odometry and lidar frames. It works like pointcloud_to_depthimage node, but at slam frequency. -->
-        <param name="gen_depth_decimation"              type="int" value="1"/><!-- Scale down image size of the camera info received (creating smaller depth image). -->
-        <param name="gen_depth_fill_holes_size"         type="int" value="0"/><!-- Fill holes of empty pixels up to this size. Values are interpolated from neighbor depth values. 0 means disabled. -->
-        <param name="gen_depth_fill_iterations"         type="double" value="0.1"/><!-- Maximum depth error (m) to interpolate. -->
-        <param name="gen_depth_fill_holes_error"        type="int" value="1"/><!-- Number of iterations to fill holes. -->
+        <param name="gen_scan"                                  type="bool"     value="false"/> <!-- Generate laser scans from depth images (using the middle horizontal line of the depth image). Not generated if subscribe_scan or subscribe_scan_cloud are true. -->
+        <param name="gen_scan_max_depth"                        type="double"   value="4.0"/><!-- Maximum depth of the laser scans generated. -->
+        <param name="gen_depth"                                 type="bool"     value="false"/><!-- Generate depth image from scan_cloud projection into RGB camera, taking into account displacement of the RGB camera accordingly to odometry and lidar frames. It works like pointcloud_to_depthimage node, but at slam frequency. -->
+        <param name="gen_depth_decimation"                      type="int"      value="1"/><!-- Scale down image size of the camera info received (creating smaller depth image). -->
+        <param name="gen_depth_fill_holes_size"                 type="int"      value="0"/><!-- Fill holes of empty pixels up to this size. Values are interpolated from neighbor depth values. 0 means disabled. -->
+        <param name="gen_depth_fill_iterations"                 type="double"   value="0.1"/><!-- Maximum depth error (m) to interpolate. -->
+        <param name="gen_depth_fill_holes_error"                type="int"      value="1"/><!-- Number of iterations to fill holes. -->
         <!-- SYNC         -->
-        <param name="odom_sensor_sync"                  type="bool" value="true"/><!-- Adjust image and scan poses relatively to odometry pose for each node added to graph. For example, if an image received at t=1s has been synchronized with a scan at t=1.1s (and our reference stamp is the scan) and the robot is moving forward at 1 m/s, then we will ask TF to know the motion between t=1s and t=1.1s (which would be 0.1 m) for the camera to adjust its local transform (-10 cm) relative to scan frame at scan stamp. This also applies to multi-camera synchronization. -->
-        <param name="approx_sync"                       type="bool" value="true"/> <!-- Use approximate time synchronization of input messages. If false, note that the odometry input must have also exactly the same timestamps than the input images. -->
+        <param name="odom_sensor_sync"                          type="bool"     value="true"/><!-- Adjust image and scan poses relatively to odometry pose for each node added to graph. For example, if an image received at t=1s has been synchronized with a scan at t=1.1s (and our reference stamp is the scan) and the robot is moving forward at 1 m/s, then we will ask TF to know the motion between t=1s and t=1.1s (which would be 0.1 m) for the camera to adjust its local transform (-10 cm) relative to scan frame at scan stamp. This also applies to multi-camera synchronization. -->
+        <param name="approx_sync"                               type="bool"     value="true"/> <!-- Use approximate time synchronization of input messages. If false, note that the odometry input must have also exactly the same timestamps than the input images. -->
         <!-- MAPPING -->
-        <param name="map_filter_radius"                         type="double" value="0.0"/><!-- Mapping Filter nodes before creating the maps. Only load data for one node in the filter radius (the latest data is used) up to filter angle (map_filter_angle). Set to 0.0 to disable node filtering. Used for all published maps. -->
-        <param name="map_filter_angle"                          type="double" value="30.0"/><!-- Mapping Angle used when filtering nodes before creating the maps. See also map_filter_radius. Used for all published maps. -->
-        <param name="map_cleanup"                               type="bool" value="true"/><!-- Mapping If there is no subscription to any map cloud_map, grid_map or proj_map, clear the corresponding data. -->
-        <param name="latch"                                     type="bool" value="true"/><!-- Mapping If true, the last message published on the map topics will be saved and sent to new subscribers when they connect. -->
-        <param name="map_always_update"                         type="bool" value="false"/><!-- Mapping Always update the occupancy grid map even if the graph has not been updated (e.g., by default when the robot is not moving, the graph is not growing). For example, if the environment is dynamic, we may want to update the map even when to robot is not moving. Another approach would be to force rtabmap to always update the graph (by setting RGBD/LinearUpdate to 0), which can be not efficient over time as the map will grow even if the robot doesn't move. -->
-        <param name="map_empty_ray_tracing"                     type="bool" value="true"/><!-- Mapping Do ray tracing to fill unknown space for invalid 2D scan's rays (assuming invalid rays to be infinite). Used only when map_always_update is also true. -->
-        <param name="cloud_output_voxelized"                    type="bool" value="true"/><!-- Mapping Do a final voxel filtering after all clouds are assembled with a voxel size equals to Grid/CellSize. Used for cloud_map published topic. -->
-        <param name="cloud_subtract_filtering"                  type="bool" value="true"/><!-- Mapping When appending a new cloud to cloud_map, filter points that have less neighbors than cloud_subtract_filtering_min_neighbors in the radius Grid/CellSize. This will helps to reconstruct the whole map' s cloud faster when a loop closure happens. Used for cloud_map published topic. -->
-        <param name="cloud_subtract_filtering_min_neighbors"    type="int" value="2"/><!-- Mapping See cloud_subtract_filtering parameter's description. -->
+        <param name="map_filter_radius"                         type="double"   value="0.0"/><!-- Mapping Filter nodes before creating the maps. Only load data for one node in the filter radius (the latest data is used) up to filter angle (map_filter_angle). Set to 0.0 to disable node filtering. Used for all published maps. -->
+        <param name="map_filter_angle"                          type="double"   value="30.0"/><!-- Mapping Angle used when filtering nodes before creating the maps. See also map_filter_radius. Used for all published maps. -->
+        <param name="map_cleanup"                               type="bool"     value="true"/><!-- Mapping If there is no subscription to any map cloud_map, grid_map or proj_map, clear the corresponding data. -->
+        <param name="latch"                                     type="bool"     value="true"/><!-- Mapping If true, the last message published on the map topics will be saved and sent to new subscribers when they connect. -->
+        <param name="map_always_update"                         type="bool"     value="false"/><!-- Mapping Always update the occupancy grid map even if the graph has not been updated (e.g., by default when the robot is not moving, the graph is not growing). For example, if the environment is dynamic, we may want to update the map even when to robot is not moving. Another approach would be to force rtabmap to always update the graph (by setting RGBD/LinearUpdate to 0), which can be not efficient over time as the map will grow even if the robot doesn't move. -->
+        <param name="map_empty_ray_tracing"                     type="bool"     value="true"/><!-- Mapping Do ray tracing to fill unknown space for invalid 2D scan's rays (assuming invalid rays to be infinite). Used only when map_always_update is also true. -->
+        <param name="cloud_output_voxelized"                    type="bool"     value="true"/><!-- Mapping Do a final voxel filtering after all clouds are assembled with a voxel size equals to Grid/CellSize. Used for cloud_map published topic. -->
+        <param name="cloud_subtract_filtering"                  type="bool"     value="true"/><!-- Mapping When appending a new cloud to cloud_map, filter points that have less neighbors than cloud_subtract_filtering_min_neighbors in the radius Grid/CellSize. This will helps to reconstruct the whole map' s cloud faster when a loop closure happens. Used for cloud_map published topic. -->
+        <param name="cloud_subtract_filtering_min_neighbors"    type="int"      value="2"/><!-- Mapping See cloud_subtract_filtering parameter's description. -->
         <!-- RGBD -->
-        <param name="rgbd_cameras" type="int" value="1"/><!-- Number of RGB-D cameras to use (when subscribe_rgbd is true). Well for now, a maximum of 4 cameras can be synchronized at the same time. If > 1, the rgbd_image topics should contain the camera index starting with 0. For example, if we have 2 cameras, you should set rgbd_image0 and rgbd_image1 topics. -->
+        <param name="rgbd_cameras"                              type="int"      value="1"/><!-- Number of RGB-D cameras to use (when subscribe_rgbd is true). Well for now, a maximum of 4 cameras can be synchronized at the same time. If > 1, the rgbd_image topics should contain the camera index starting with 0. For example, if we have 2 cameras, you should set rgbd_image0 and rgbd_image1 topics. -->
 
         
-        <param name="use_action_for_goal" type="bool" value="true"/><!-- Planning Use actionlib to send the metric goals to move_base. The advantage over just connecting goal_out to move_base_simple/ goal is that rtabmap can have a feedback if the goal is reached or if move_base has failed. See move_base Action API for more info. -->
+        <param name="use_action_for_goal"                       type="bool"     value="true"/><!-- Planning Use actionlib to send the metric goals to move_base. The advantage over just connecting goal_out to move_base_simple/ goal is that rtabmap can have a feedback if the goal is reached or if move_base has failed. See move_base Action API for more info. -->
     </node>
 </group>
 <node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan">
-    <param name="scan_height" value="0.07" />
-    <param name="output_frame_id" value="$(arg camera2)_link" />
-    <param name="range_min" type="double" value="0.1" />
-    <param name="range_max" type="double" value="4.0" />
-    <remap from="image" to="/$(arg camera2)/aligned_depth_to_color/image_raw" />
-    <remap from="scan" to="/$(arg camera2)/scan" />
+    <param name="scan_height"       value="0.07" />
+    <param name="output_frame_id"   value="$(arg camera2)_link" />
+    <param name="range_min"         type="double" value="0.1" />
+    <param name="range_max"         type="double" value="4.0" />
+    <remap from="image"             to="/$(arg camera2)/aligned_depth_to_color/image_raw" />
+    <remap from="scan"              to="/$(arg camera2)/scan" />
 </node>
 
 </launch>
\ No newline at end of file

From b2b414992fcfdc976dfde559306b4a61d530788e Mon Sep 17 00:00:00 2001
From: IvanSnap360 <ivpetr2003@gmail.com>
Date: Mon, 5 Jun 2023 08:14:35 +0300
Subject: [PATCH 04/13] att d435 to manipulator

---
 src/omnibot_bringup/rviz/rviz.rviz            | 172 +++++++++++++++++-
 .../launch/omnibot_gazebo.launch              |   8 +-
 .../rviz/rviz.rviz                            |  88 +++++----
 .../urdf/omnibot_manipulator.urdf.xacro       |  31 +++-
 .../config/sensors_3d.yaml                    |  19 +-
 .../launch/sensor_manager.launch.xml          |   2 +-
 .../launch/rtabmap.launch                     |   4 +-
 7 files changed, 259 insertions(+), 65 deletions(-)

diff --git a/src/omnibot_bringup/rviz/rviz.rviz b/src/omnibot_bringup/rviz/rviz.rviz
index 3a41888..f438df7 100644
--- a/src/omnibot_bringup/rviz/rviz.rviz
+++ b/src/omnibot_bringup/rviz/rviz.rviz
@@ -6,13 +6,14 @@ Panels:
       Expanded:
         - /Global Options1
         - /Status1
+        - /3DMap1
         - /Robot1/LaserScan1
         - /move_base1/TEB_planner1
         - /move_base1/CostmapPlanner1/cost_cloud1
         - /move_base1/RRT_Global_Planner1/global_plan1
         - /move_base1/EbandPlanner1
       Splitter Ratio: 0.5080732107162476
-    Tree Height: 1656
+    Tree Height: 758
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -83,6 +84,26 @@ Visualization Manager:
           Value: true
         base_link:
           Value: true
+        d400_arm_bottom_screw_frame:
+          Value: true
+        d400_arm_color_frame:
+          Value: true
+        d400_arm_color_optical_frame:
+          Value: true
+        d400_arm_depth_frame:
+          Value: true
+        d400_arm_depth_optical_frame:
+          Value: true
+        d400_arm_left_ir_frame:
+          Value: true
+        d400_arm_left_ir_optical_frame:
+          Value: true
+        d400_arm_link:
+          Value: true
+        d400_arm_right_ir_frame:
+          Value: true
+        d400_arm_right_ir_optical_frame:
+          Value: true
         d400_bottom_screw_frame:
           Value: true
         d400_color_frame:
@@ -277,6 +298,20 @@ Visualization Manager:
                             {}
                           ang_link7:
                             {}
+                          d400_arm_bottom_screw_frame:
+                            d400_arm_link:
+                              d400_arm_depth_frame:
+                                d400_arm_color_frame:
+                                  d400_arm_color_optical_frame:
+                                    {}
+                                d400_arm_depth_optical_frame:
+                                  {}
+                                d400_arm_left_ir_frame:
+                                  d400_arm_left_ir_optical_frame:
+                                    {}
+                                d400_arm_right_ir_frame:
+                                  d400_arm_right_ir_optical_frame:
+                                    {}
                           tip_link:
                             {}
               base_link:
@@ -606,6 +641,47 @@ Visualization Manager:
               Show Axes: false
               Show Trail: false
               Value: true
+            d400_arm_bottom_screw_frame:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            d400_arm_color_frame:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            d400_arm_color_optical_frame:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            d400_arm_depth_frame:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            d400_arm_depth_optical_frame:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            d400_arm_left_ir_frame:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            d400_arm_left_ir_optical_frame:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            d400_arm_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+              Value: true
+            d400_arm_right_ir_frame:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            d400_arm_right_ir_optical_frame:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
             d400_bottom_screw_frame:
               Alpha: 1
               Show Axes: false
@@ -1510,7 +1586,7 @@ Visualization Manager:
       Name: move_base
     - Acceleration_Scaling_Factor: 0.1
       Class: moveit_rviz_plugin/MotionPlanning
-      Enabled: false
+      Enabled: true
       Move Group Namespace: ""
       MoveIt_Allow_Approximate_IK: false
       MoveIt_Allow_External_Program: true
@@ -1588,6 +1664,47 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
+          d400_arm_bottom_screw_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_color_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_color_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_depth_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_depth_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_left_ir_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_left_ir_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          d400_arm_right_ir_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_right_ir_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           d400_bottom_screw_frame:
             Alpha: 1
             Show Axes: false
@@ -2089,6 +2206,47 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
+          d400_arm_bottom_screw_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_color_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_color_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_depth_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_depth_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_left_ir_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_left_ir_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          d400_arm_right_ir_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_right_ir_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           d400_bottom_screw_frame:
             Alpha: 1
             Show Axes: false
@@ -2498,7 +2656,7 @@ Visualization Manager:
         Robot Alpha: 1
         Show Robot Collision: false
         Show Robot Visual: false
-      Value: false
+      Value: true
       Velocity_Scaling_Factor: 0.1
   Enabled: true
   Global Options:
@@ -2528,7 +2686,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 5.916483402252197
+      Distance: 4.92646598815918
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -2544,9 +2702,9 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5653976798057556
+      Pitch: 0.5503977537155151
       Target Frame: base_link
-      Yaw: 2.9935927391052246
+      Yaw: 1.573593020439148
     Saved: ~
 Window Geometry:
   Displays:
@@ -2560,7 +2718,7 @@ Window Geometry:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
diff --git a/src/omnibot_gazebo/launch/omnibot_gazebo.launch b/src/omnibot_gazebo/launch/omnibot_gazebo.launch
index ad36b3e..9813b28 100644
--- a/src/omnibot_gazebo/launch/omnibot_gazebo.launch
+++ b/src/omnibot_gazebo/launch/omnibot_gazebo.launch
@@ -30,18 +30,18 @@
   <arg name="z_pos" default="0.3"/> -->
 
   <!-- MyWorld -->
-  <arg name="x_pos" default="-4.0"/>
+  <!-- <arg name="x_pos" default="-4.0"/>
   <arg name="y_pos" default="3.0"/>
-  <arg name="z_pos" default="0.3"/>
+  <arg name="z_pos" default="0.3"/> -->
   <!-- 
   <arg name="x_pos" default="0.0"/>
   <arg name="y_pos" default="-2.65"/>
   <arg name="z_pos" default="0.3"/> -->
 
-<!-- 
+
   <arg name="x_pos" default="0.0" />
   <arg name="y_pos" default="1.0" />
-  <arg name="z_pos" default="0.3" /> -->
+  <arg name="z_pos" default="0.3" />
 
 
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
diff --git a/src/omnibot_manipulator/omnibot_manipulator_description/rviz/rviz.rviz b/src/omnibot_manipulator/omnibot_manipulator_description/rviz/rviz.rviz
index 3010713..58b6827 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_description/rviz/rviz.rviz
+++ b/src/omnibot_manipulator/omnibot_manipulator_description/rviz/rviz.rviz
@@ -102,6 +102,47 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        d400_arm_bottom_screw_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d400_arm_color_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d400_arm_color_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d400_arm_depth_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d400_arm_depth_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d400_arm_left_ir_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d400_arm_left_ir_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d400_arm_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        d400_arm_right_ir_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d400_arm_right_ir_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
         tip_link:
           Alpha: 1
           Show Axes: false
@@ -117,32 +158,12 @@ Visualization Manager:
       Value: true
       Visual Enabled: true
     - Class: rviz/TF
-      Enabled: true
+      Enabled: false
+      Filter (blacklist): ""
+      Filter (whitelist): ""
       Frame Timeout: 15
       Frames:
         All Enabled: true
-        ang_link0:
-          Value: true
-        ang_link1:
-          Value: true
-        ang_link2:
-          Value: true
-        ang_link3:
-          Value: true
-        ang_link4:
-          Value: true
-        ang_link5:
-          Value: true
-        ang_link6:
-          Value: true
-        ang_link7:
-          Value: true
-        map:
-          Value: true
-        tip_link:
-          Value: true
-        world:
-          Value: true
       Marker Alpha: 0.5
       Marker Scale: 0.30000001192092896
       Name: TF
@@ -150,22 +171,9 @@ Visualization Manager:
       Show Axes: true
       Show Names: true
       Tree:
-        map:
-          world:
-            ang_link0:
-              ang_link1:
-                ang_link2:
-                  ang_link3:
-                    ang_link4:
-                      ang_link5:
-                        ang_link6:
-                          {}
-                        ang_link7:
-                          {}
-                        tip_link:
-                          {}
+        {}
       Update Interval: 0
-      Value: true
+      Value: false
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -210,9 +218,9 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.73539799451828
+      Pitch: 0.19539840519428253
       Target Frame: <Fixed Frame>
-      Yaw: 5.278582572937012
+      Yaw: 6.193588733673096
     Saved: ~
 Window Geometry:
   Displays:
diff --git a/src/omnibot_manipulator/omnibot_manipulator_description/urdf/omnibot_manipulator.urdf.xacro b/src/omnibot_manipulator/omnibot_manipulator_description/urdf/omnibot_manipulator.urdf.xacro
index f831f07..19d3732 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_description/urdf/omnibot_manipulator.urdf.xacro
+++ b/src/omnibot_manipulator/omnibot_manipulator_description/urdf/omnibot_manipulator.urdf.xacro
@@ -13,6 +13,19 @@
 		</gazebo>
 	</xacro:unless>
 
+
+	<xacro:property name="d435_xacro_path"
+		value="$(find realsense_gazebo_description)/urdf/_d435.urdf.xacro" />
+    <xacro:include filename="${d435_xacro_path}" />
+
+	<xacro:property name="d435_x_offset" value="0.04" />
+	<xacro:property name="d435_y_offset" value="0.0" />
+	<xacro:property name="d435_z_offset" value="-0.023" />
+	<xacro:property name="d435_x_rotate" value="0.0" />
+	<xacro:property name="d435_y_rotate" value="1.57" />
+	<xacro:property name="d435_z_rotate" value="0.0" />
+
+
 	<xacro:include
 		filename="$(find omnibot_manipulator_description)/urdf/omnibot_manipulator.gazebo" />
 
@@ -223,8 +236,6 @@
 			velocity="5.0" />
 	</joint>
 
-	
-	
 
 	<link name="ang_link5">
 		<visual>
@@ -249,13 +260,13 @@
 		</collision>
 	</link>
 
-	<link name="tip_link"/>
+	<link name="tip_link" />
 	<joint name="tip_joint" type="fixed">
-		<parent link="ang_link5"/>
-		<child link="tip_link"/>
-		<origin xyz="0.0 0.0 -0.118" rpy="0.0 0.0 0.0"/>
+		<parent link="ang_link5" />
+		<child link="tip_link" />
+		<origin xyz="0.0 0.0 -0.118" rpy="0.0 0.0 0.0" />
 	</joint>
-	
+
 
 	<joint name="ang_gripper" type="prismatic">
 		<parent link="ang_link5" />
@@ -319,4 +330,10 @@
 				izz="0.01" />
 		</inertial>
 	</link>
+
+
+	<xacro:sensor_d435 parent="ang_link5" name="d400_arm" topics_ns="d400_arm" align_depth="true"
+		enable_pointCloud="true">
+		<origin xyz="${d435_x_offset} ${d435_y_offset}  ${d435_z_offset}" rpy="${d435_x_rotate} ${d435_y_rotate} ${d435_z_rotate}" />
+	</xacro:sensor_d435>
 </robot>
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/config/sensors_3d.yaml b/src/omnibot_manipulator/omnibot_manipulator_moveit/config/sensors_3d.yaml
index fb7c133..35db251 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/config/sensors_3d.yaml
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/config/sensors_3d.yaml
@@ -1,15 +1,26 @@
 # Params for the octomap monitor
 
-
+# sensors:
+  # - sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
+  #   image_topic: /d400_arm/aligned_depth_to_color/image_raw
+  #   queue_size: 5
+  #   near_clipping_plane_distance: 0.01
+  #   far_clipping_plane_distance: 5.0
+  #   shadow_threshold: 0.2
+  #   padding_scale: 1.0
+  #   padding_offset: 0.05
+  #   max_update_rate: 10.0
+  #   filtered_cloud_topic: filtered_cloud
+  #   ns: d400_arm
 
 sensors: 
   - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
     # point_cloud_topic: /rtabmap/cloud_map
     # point_cloud_topic: /rtabmap/cloud_obstacles
-    point_cloud_topic: /rtabmap/local_grid_obstacle
-    # point_cloud_topic: /d400/depth/color/points
+    # point_cloud_topic: /rtabmap/local_grid_obstacle
+    point_cloud_topic: /d400_arm/depth/color/points
     max_range: 2.0
-    max_update_rate: 100.0
+    max_update_rate: 30.0
     point_subsample: 1
     padding_offset: 0.05
     padding_scale: 1.0
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/sensor_manager.launch.xml b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/sensor_manager.launch.xml
index c748c72..3315168 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/sensor_manager.launch.xml
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/sensor_manager.launch.xml
@@ -5,7 +5,7 @@
   <!-- Params for 3D sensors config -->
   <rosparam command="load" file="$(find omnibot_manipulator_moveit)/config/sensors_3d.yaml" />
   <!-- Params for the octomap monitor -->
-  <param name="octomap_frame" type="string" value="odom" />
+  <param name="octomap_frame" type="string" value="d400_arm" />
   <param name="octomap_resolution" type="double" value="0.1" /> <!-- in meters -->
   <param name="max_range" type="double" value="2.5" />
 
diff --git a/src/omnibot_platform/omnibot_platform_navigation/launch/rtabmap.launch b/src/omnibot_platform/omnibot_platform_navigation/launch/rtabmap.launch
index 85e0cbe..bcb99fc 100644
--- a/src/omnibot_platform/omnibot_platform_navigation/launch/rtabmap.launch
+++ b/src/omnibot_platform/omnibot_platform_navigation/launch/rtabmap.launch
@@ -72,12 +72,12 @@
         <!-- ################################################### -->
         <param name="Icp/CorrespondenceRatio"      type="string" value="0.3"/>
         <!-- ################################################### -->
-        <param name="Vis/MinInliers"                            type="string"   value="15" /><!-- 3D visual words minimum inliers to accept loop closure -->
+        <param name="Vis/MinInliers"                            type="string"   value="10" /><!-- 3D visual words minimum inliers to accept loop closure -->
         <param name="Vis/InlierDistance"                        type="string"   value="0.1" /> <!-- 3D visual words correspondence distance -->
         <!-- ################################################### -->
         <param name="Optimizer/Slam2D"                          type="string"   value="true"/>
         <!-- ################################################### -->
-        <param name="Odom/Strategy"                             type="string"   value="5"/> <!-- 0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D-->
+        <param name="Odom/Strategy"                             type="string"   value="0"/> <!-- 0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D-->
         <param name="Odom/Holonomic"		                    type="string"   value="true"/> <!-- If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).-->
         <param name="Odom/FilteringStrategy"                    type="string"   value="0"/> <!-- 0=No filtering 1=Kalman filtering 2=Particle filtering. This filter is used to smooth the odometry output.-->
 <!-- ################################################################################################################################################################# -->

From 11475df4452dc5c30db22d97663450c01adcf8bc Mon Sep 17 00:00:00 2001
From: IvanSnap360 <ivpetr2003@gmail.com>
Date: Fri, 9 Jun 2023 20:00:12 +0300
Subject: [PATCH 05/13] real manipulator works with moveit

---
 ...omnibot_manipulator_control_server_lib.cpp |   2 +-
 .../CMakeLists.txt                            | 215 +++++++++++++++++
 .../config/dyn_config.yaml                    |  31 +++
 .../omnibot_manipulator_hw_controller_lib.h   |   0
 .../launch/dynamixel.launch                   |  23 ++
 .../omnibot_manipulator_hw_controller.launch  |  12 +
 .../package.xml                               |  75 ++++++
 .../src/action_server.cpp                     |  57 +++++
 .../omnibot_manipulator_hw_controller.cpp     |   0
 ...omnibot_manipulator_hw_controller_node.cpp |  38 +++
 .../.setup_assistant                          |   2 +-
 .../omnibot_manipulator_moveit/CMakeLists.txt |   2 +-
 .../config/controllers.yaml                   |   6 +
 .../config/gazebo_omnibot_robot.urdf          | 221 ++++++++++++++++++
 .../config/ros_controllers.yaml               |  11 +-
 .../config/sensors_3d.yaml                    |  29 +--
 .../config/simple_moveit_controllers.yaml     |   4 +-
 .../launch/demo.launch                        |  22 +-
 .../launch/gazebo.launch                      |   8 +-
 .../launch/joystick_control.launch            |   6 +
 .../launch/move_group.launch                  |   2 -
 ...bot_robot_moveit_sensor_manager.launch.xml |   3 +-
 ...ntrol_moveit_controller_manager.launch.xml |   5 +-
 .../launch/sensor_manager.launch.xml          |  10 +-
 .../launch/rtabmap.launch                     |  10 +-
 25 files changed, 726 insertions(+), 68 deletions(-)
 create mode 100644 src/omnibot_manipulator/omnibot_manipulator_hw_controller/CMakeLists.txt
 create mode 100644 src/omnibot_manipulator/omnibot_manipulator_hw_controller/config/dyn_config.yaml
 create mode 100644 src/omnibot_manipulator/omnibot_manipulator_hw_controller/include/omnibot_manipulator_hw_controller_lib/omnibot_manipulator_hw_controller_lib.h
 create mode 100644 src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/dynamixel.launch
 create mode 100644 src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/omnibot_manipulator_hw_controller.launch
 create mode 100644 src/omnibot_manipulator/omnibot_manipulator_hw_controller/package.xml
 create mode 100644 src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/action_server.cpp
 create mode 100644 src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/omnibot_manipulator_hw_controller_lib/omnibot_manipulator_hw_controller.cpp
 create mode 100644 src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/omnibot_manipulator_hw_controller_node.cpp
 create mode 100644 src/omnibot_manipulator/omnibot_manipulator_moveit/config/controllers.yaml

diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp b/src/omnibot_manipulator/omnibot_manipulator_control/src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp
index 082719a..fd4b29b 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_control/src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp
@@ -167,7 +167,7 @@ bool OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::ManipMoveByJointValues(std::vector<
              joint_values[0], joint_values[1], joint_values[2], joint_values[3], joint_values[4]);
     updateGoalState();
     bool success = (_move_group_interface_arm->plan(_arm_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
-
+    // _arm_plan.trajectory_.joint_trajectory.
     ROS_INFO("Planning %s", success ? "SUCCESS" : "FAILED");
 
     if (success)
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/CMakeLists.txt b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/CMakeLists.txt
new file mode 100644
index 0000000..8ba3205
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/CMakeLists.txt
@@ -0,0 +1,215 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(omnibot_manipulator_hw_controller)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+  actionlib_msgs
+  actionlib
+  message_generation
+
+)
+
+## System dependencies are found with CMake's conventions
+find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+  actionlib_msgs
+)
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+ INCLUDE_DIRS include
+ LIBRARIES omnibot_manipulator_hw_controller
+ CATKIN_DEPENDS roscpp rospy std_msgs actionlib_msgs actionlib
+#  DEPENDS system_lib 
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/omnibot_manipulator_hw_controller.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+add_executable(${PROJECT_NAME}_node src/omnibot_manipulator_hw_controller_node.cpp)
+add_executable(action_server  src/action_server.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}_node
+  ${catkin_LIBRARIES}
+)
+target_link_libraries(action_server
+  ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_omnibot_manipulator_hw_controller.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/config/dyn_config.yaml b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/config/dyn_config.yaml
new file mode 100644
index 0000000..060bd12
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/config/dyn_config.yaml
@@ -0,0 +1,31 @@
+ang_joint1:
+  ID: 1
+  Return_Delay_Time: 0
+  CW_Angle_Limit: 0
+  CCW_Angle_Limit: 4095
+ang_joint2:
+  ID: 2
+  Return_Delay_Time: 0
+  CW_Angle_Limit: 1023
+  CCW_Angle_Limit: 3072
+ang_joint3:
+  ID: 3
+  Return_Delay_Time: 0
+  CW_Angle_Limit: 1023
+  CCW_Angle_Limit: 3072
+ang_joint4:
+  ID: 4
+  Return_Delay_Time: 0
+  CW_Angle_Limit: 1023
+  CCW_Angle_Limit: 3072
+ang_joint5:
+  ID: 5
+  Return_Delay_Time: 0
+  CW_Angle_Limit: 0
+  CCW_Angle_Limit: 1023
+
+ang_gripper:
+  ID: 6
+  Return_Delay_Time: 0
+  CW_Angle_Limit: 0
+  CCW_Angle_Limit: 1023
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/include/omnibot_manipulator_hw_controller_lib/omnibot_manipulator_hw_controller_lib.h b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/include/omnibot_manipulator_hw_controller_lib/omnibot_manipulator_hw_controller_lib.h
new file mode 100644
index 0000000..e69de29
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/dynamixel.launch b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/dynamixel.launch
new file mode 100644
index 0000000..6028d87
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/dynamixel.launch
@@ -0,0 +1,23 @@
+<launch>
+  <arg name="usb_port"                default="/dev/ttyACM0"/>
+  <arg name="dxl_baud_rate"           default="1000000"/>
+  <arg name="namespace"               default="dynamixel_contoller"/>
+
+  <arg name="use_moveit"              default="true"/>
+  <arg name="use_joint_state"         default="true"/>
+  <arg name="use_cmd_vel"             default="false"/>
+
+  <param name="dynamixel_info"          value="$(find omnibot_manipulator_hw_controller)/config/dyn_config.yaml"/>
+
+  <node name="$(arg namespace)" pkg="dynamixel_workbench_controllers" type="dynamixel_workbench_controllers"
+        required="true" output="screen" args="$(arg usb_port) $(arg dxl_baud_rate)">
+    <param name="use_moveit"              value="$(arg use_moveit)"/>
+    <param name="use_joint_states_topic"  value="$(arg use_joint_state)"/>
+    <param name="use_cmd_vel_topic"       value="$(arg use_cmd_vel)"/>
+    <rosparam>
+      publish_period: 0.001
+      dxl_read_period: 0.001
+      dxl_write_period: 0.001
+    </rosparam>
+  </node>
+</launch>
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/omnibot_manipulator_hw_controller.launch b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/omnibot_manipulator_hw_controller.launch
new file mode 100644
index 0000000..1790061
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/omnibot_manipulator_hw_controller.launch
@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<launch>
+    <include file="$(find omnibot_manipulator_hw_controller)/launch/dynamixel.launch">
+        <arg name="usb_port" default="/dev/ttyACM0" />
+        <arg name="dxl_baud_rate" default="1000000" />
+        <arg name="namespace" default="dynamixel_contoller" />
+    </include>
+    <node pkg="omnibot_manipulator_hw_controller" type="omnibot_manipulator_hw_controller_node"
+        name="omnibot_manipulator_hw_controller_node" output="screen" />
+    <node pkg="omnibot_manipulator_hw_controller" type="action_server"
+        name="omnibot_manipulator_hw_controller_action_server" output="screen" />
+</launch>
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/package.xml b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/package.xml
new file mode 100644
index 0000000..34a6dc5
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/package.xml
@@ -0,0 +1,75 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>omnibot_manipulator_hw_controller</name>
+  <version>0.0.0</version>
+  <description>The omnibot_manipulator_hw_controller package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="ivan@todo.todo">ivan</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/omnibot_manipulator_hw_controller</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>message_generation</exec_depend>
+  <exec_depend>actionlib_msgs</exec_depend>
+  <build_export_depend>actionlib_msgs</build_export_depend>
+  <build_depend>actionlib_msgs</build_depend>
+  <build_depend>actionlib</build_depend>
+  <build_export_depend>actionlib</build_export_depend>
+  <exec_depend>actionlib</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/action_server.cpp b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/action_server.cpp
new file mode 100644
index 0000000..f9d3ecf
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/action_server.cpp
@@ -0,0 +1,57 @@
+#include <ros/ros.h>
+#include <actionlib/server/simple_action_server.h>
+#include <control_msgs/FollowJointTrajectoryAction.h>
+#include <trajectory_msgs/JointTrajectory.h>
+
+class RobotTrajectoryFollower
+{
+protected:
+    ros::NodeHandle nh_;
+    // NodeHandle instance must be created before this line. Otherwise strange error may occur.
+    actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_;
+    ros::Publisher _trajectory_publisher;
+    std::string action_name_;
+
+public:
+    RobotTrajectoryFollower(std::string name,std::string traj_pub_name) : as_(nh_, name, false),
+                                                action_name_(name)
+    {
+        // Register callback functions:
+        as_.registerGoalCallback(boost::bind(&RobotTrajectoryFollower::goalCB, this));
+        as_.registerPreemptCallback(boost::bind(&RobotTrajectoryFollower::preemptCB, this));
+
+        as_.start();
+        _trajectory_publisher = nh_.advertise<trajectory_msgs::JointTrajectory>(traj_pub_name,10);
+    }
+
+    ~RobotTrajectoryFollower(void) // Destructor
+    {
+    }
+
+    void goalCB()
+    {
+        // accept the new goal
+        // goal_ = as_.acceptNewGoal()->samples;
+        std::cout << "goalCB!!!" << std::endl;
+        _trajectory_publisher.publish(as_.acceptNewGoal()->trajectory);
+    }
+
+    void preemptCB()
+    {
+        ROS_INFO("%s: Preempted", action_name_.c_str());
+        // set the action state to preempted
+        as_.setPreempted();
+    }
+};
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "action_server");
+
+    RobotTrajectoryFollower ManipulatorTrajectoryFollower("/maipulator_group_controller/follow_joint_trajectory","/dynamixel_contoller/joint_trajectory");
+    RobotTrajectoryFollower GripperTrajectoryFollower("/gripper_group_controller/follow_joint_trajectory","/dynamixel_contoller/joint_trajectory");
+
+    ros::spin();
+
+    return 0;
+}
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/omnibot_manipulator_hw_controller_lib/omnibot_manipulator_hw_controller.cpp b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/omnibot_manipulator_hw_controller_lib/omnibot_manipulator_hw_controller.cpp
new file mode 100644
index 0000000..e69de29
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/omnibot_manipulator_hw_controller_node.cpp b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/omnibot_manipulator_hw_controller_node.cpp
new file mode 100644
index 0000000..0f819a0
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/omnibot_manipulator_hw_controller_node.cpp
@@ -0,0 +1,38 @@
+#include <ros/ros.h>
+#include <trajectory_msgs/JointTrajectory.h>
+#include "sensor_msgs/JointState.h"
+#include <control_msgs/FollowJointTrajectoryAction.h>
+
+
+ros::Subscriber dynamixel_controller_joint_state_subscriber;
+ros::Publisher manipulator_joint_state_publisher;
+
+
+float map(float x, float in_min, float in_max, float out_min, float out_max)
+{
+    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+void dynamixel_controller_joint_state_subscriber_cb_f(const sensor_msgs::JointState::ConstPtr &in_data)
+{
+    static sensor_msgs::JointState out_data;
+    out_data = *in_data;
+    out_data.position[0] = map(in_data->position[0], -0.4, 0.8, 0.0, 0.025);
+    manipulator_joint_state_publisher.publish(out_data);
+    // std::cout << "get joint_state" << std::endl;
+}
+
+int main(int argc, char *argv[])
+{
+    ros::init(argc, argv, "omnibotmanipulator_hw_controller_node");
+    ros::NodeHandle nh;
+
+    dynamixel_controller_joint_state_subscriber =
+        nh.subscribe<sensor_msgs::JointState>("/dynamixel_contoller/joint_states", 10,
+                                              &dynamixel_controller_joint_state_subscriber_cb_f);
+
+    manipulator_joint_state_publisher = nh.advertise<sensor_msgs::JointState>("/omnibot_robot/manipulator_joint_states_correcter", 10);
+    ros::spin();
+    ros::shutdown();
+    return 0;
+}
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/.setup_assistant b/src/omnibot_manipulator/omnibot_manipulator_moveit/.setup_assistant
index 8d3f1b3..313f8ba 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/.setup_assistant
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/.setup_assistant
@@ -8,4 +8,4 @@ moveit_setup_assistant_config:
   CONFIG:
     author_name: __Druid__
     author_email: oldsnapdruid@gmail.com
-    generated_timestamp: 1677949637
\ No newline at end of file
+    generated_timestamp: 1686319855
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/CMakeLists.txt b/src/omnibot_manipulator/omnibot_manipulator_moveit/CMakeLists.txt
index 4ab75d4..dd1efc7 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/CMakeLists.txt
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/CMakeLists.txt
@@ -7,4 +7,4 @@ catkin_package()
 
 install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
   PATTERN "setup_assistant.launch" EXCLUDE)
-install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
\ No newline at end of file
+install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/config/controllers.yaml b/src/omnibot_manipulator/omnibot_manipulator_moveit/config/controllers.yaml
new file mode 100644
index 0000000..694ab23
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/config/controllers.yaml
@@ -0,0 +1,6 @@
+controller_list:
+  - name: maipulator_arm_contoller
+    action_ns: joint_trajectory_controller
+    type: FollowJointTrajectory
+    default: true
+    joints: [ang_joint1, ang_joint2, ang_joint3, ang_joint4, ang_joint5]
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/config/gazebo_omnibot_robot.urdf b/src/omnibot_manipulator/omnibot_manipulator_moveit/config/gazebo_omnibot_robot.urdf
index efad880..91803b7 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/config/gazebo_omnibot_robot.urdf
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/config/gazebo_omnibot_robot.urdf
@@ -4570,6 +4570,227 @@
             <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
         </inertial>
     </link>
+    <material name="d400_arm_aluminum">
+        <color rgba="0.5 0.5 0.5 1" />
+    </material>
+    <!-- camera body, with origin at bottom screw mount -->
+    <joint name="d400_arm_joint" type="fixed">
+        <origin rpy="0.0 1.57 0.0" xyz="0.04 0.0  -0.023" />
+        <parent link="ang_link5" />
+        <child link="d400_arm_bottom_screw_frame" />
+    </joint>
+    <link name="d400_arm_bottom_screw_frame" />
+    <joint name="d400_arm_link_joint" type="fixed">
+        <origin rpy="0 0 0" xyz="0 0.0175 0.0125" />
+        <parent link="d400_arm_bottom_screw_frame" />
+        <child link="d400_arm_link" />
+    </joint>
+    <link name="d400_arm_link">
+        <visual>
+            <origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0.0149 -0.0175 0" />
+            <geometry>
+                <!-- <box size="${d435_cam_width} ${d435_cam_height} ${d435_cam_depth}"/> -->
+                <mesh filename="package://realsense2_description/meshes/d435.dae" />
+                <!--<mesh filename="package://realsense2_description/meshes/d435/d435.dae"/>-->
+            </geometry>
+            <material name="d400_arm_aluminum" />
+        </visual>
+        <collision>
+            <origin rpy="0 0 0" xyz="0 -0.0175 0" />
+            <geometry>
+                <box size="0.02505 0.09 0.025" />
+            </geometry>
+        </collision>
+        <inertial>
+            <!-- The following are not reliable values, and should not be used for modeling -->
+            <mass value="0.564" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <inertia ixx="0.003881243" ixy="0.0" ixz="0.0" iyy="0.000498940" iyz="0.0" izz="0.003879257" />
+        </inertial>
+    </link>
+    <!-- camera depth joints and links -->
+    <joint name="d400_arm_depth_joint" type="fixed">
+        <origin rpy="0 0 0" xyz="0 0 0" />
+        <parent link="d400_arm_link" />
+        <child link="d400_arm_depth_frame" />
+    </joint>
+    <link name="d400_arm_depth_frame" />
+    <joint name="d400_arm_depth_optical_joint" type="fixed">
+        <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0" />
+        <parent link="d400_arm_depth_frame" />
+        <child link="d400_arm_depth_optical_frame" />
+    </joint>
+    <link name="d400_arm_depth_optical_frame" />
+    <!-- camera left IR joints and links -->
+    <joint name="d400_arm_left_ir_joint" type="fixed">
+        <origin rpy="0 0 0" xyz="0 0.0 0" />
+        <parent link="d400_arm_depth_frame" />
+        <child link="d400_arm_left_ir_frame" />
+    </joint>
+    <link name="d400_arm_left_ir_frame" />
+    <joint name="d400_arm_left_ir_optical_joint" type="fixed">
+        <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0" />
+        <parent link="d400_arm_left_ir_frame" />
+        <child link="d400_arm_left_ir_optical_frame" />
+    </joint>
+    <link name="d400_arm_left_ir_optical_frame" />
+    <!-- camera right IR joints and links -->
+    <joint name="d400_arm_right_ir_joint" type="fixed">
+        <origin rpy="0 0 0" xyz="0 -0.05 0" />
+        <parent link="d400_arm_depth_frame" />
+        <child link="d400_arm_right_ir_frame" />
+    </joint>
+    <link name="d400_arm_right_ir_frame" />
+    <joint name="d400_arm_right_ir_optical_joint" type="fixed">
+        <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0" />
+        <parent link="d400_arm_right_ir_frame" />
+        <child link="d400_arm_right_ir_optical_frame" />
+    </joint>
+    <link name="d400_arm_right_ir_optical_frame" />
+    <!-- camera color joints and links -->
+    <joint name="d400_arm_color_joint" type="fixed">
+        <origin rpy="0 0 0" xyz="0 0.015 0" />
+        <parent link="d400_arm_depth_frame" />
+        <child link="d400_arm_color_frame" />
+    </joint>
+    <link name="d400_arm_color_frame" />
+    <joint name="d400_arm_color_optical_joint" type="fixed">
+        <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0" />
+        <parent link="d400_arm_color_frame" />
+        <child link="d400_arm_color_optical_frame" />
+    </joint>
+    <link name="d400_arm_color_optical_frame" />
+    <gazebo reference="d400_arm_link">
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+        <!-- <gravity>1</gravity> -->
+        <!--<mu>1</mu>-->
+        <mu2>1</mu2>
+        <fdir1>0 0 0</fdir1>
+        <!--<slip1>0</slip1>
+      <slip2>0</slip2>-->
+        <kp>1e+13</kp>
+        <kd>1</kd>
+        <!--<max_vel>0.01</max_vel>
+      <min_depth>0</min_depth>-->
+        <sensor name="d400_armcolor" type="camera">
+            <camera name="d400_arm">
+                <horizontal_fov>1.2112585008840648</horizontal_fov>
+                <image>
+                    <width>1920</width>
+                    <height>1080</height>
+                    <format>RGB_INT8</format>
+                </image>
+                <clip>
+                    <near>0.1</near>
+                    <far>100</far>
+                </clip>
+                <noise>
+                    <type>gaussian</type>
+                    <mean>0.0</mean>
+                    <stddev>0.007</stddev>
+                </noise>
+            </camera>
+            <always_on>1</always_on>
+            <update_rate>30</update_rate>
+            <visualize>False</visualize>
+        </sensor>
+        <sensor name="d400_armired1" type="camera">
+            <camera name="d400_arm">
+                <horizontal_fov>1.4870205226991688</horizontal_fov>
+                <image>
+                    <width>640</width>
+                    <height>480</height>
+                    <format>L_INT8</format>
+                </image>
+                <clip>
+                    <near>0.1</near>
+                    <far>100</far>
+                </clip>
+                <noise>
+                    <type>gaussian</type>
+                    <mean>0.0</mean>
+                    <stddev>0.05</stddev>
+                </noise>
+            </camera>
+            <always_on>1</always_on>
+            <update_rate>30</update_rate>
+            <visualize>false</visualize>
+        </sensor>
+        <sensor name="d400_armired2" type="camera">
+            <camera name="d400_arm">
+                <horizontal_fov>1.4870205226991688</horizontal_fov>
+                <image>
+                    <width>640</width>
+                    <height>480</height>
+                    <format>L_INT8</format>
+                </image>
+                <clip>
+                    <near>0.1</near>
+                    <far>100</far>
+                </clip>
+                <noise>
+                    <type>gaussian</type>
+                    <mean>0.0</mean>
+                    <stddev>0.05</stddev>
+                </noise>
+            </camera>
+            <always_on>1</always_on>
+            <update_rate>30</update_rate>
+            <visualize>false</visualize>
+        </sensor>
+        <sensor name="d400_armdepth" type="depth">
+            <camera name="d400_arm">
+                <horizontal_fov>1.2112585008840648</horizontal_fov>
+                <image>
+                    <width>1920</width>
+                    <height>1080</height>
+                </image>
+                <clip>
+                    <near>0.1</near>
+                    <far>100</far>
+                </clip>
+                <noise>
+                    <type>gaussian</type>
+                    <mean>0.0</mean>
+                    <stddev>0.100</stddev>
+                </noise>
+            </camera>
+            <always_on>1</always_on>
+            <update_rate>30</update_rate>
+            <visualize>false</visualize>
+        </sensor>
+    </gazebo>
+    <gazebo>
+        <plugin filename="librealsense_gazebo_plugin.so" name="d400_arm">
+            <prefix>d400_arm</prefix>
+            <!-- Color camera settings -->
+            <colorUpdateRate>30</colorUpdateRate>
+            <colorTopicName>color/image_raw</colorTopicName>
+            <colorOpticalframeName>d400_arm_color_optical_frame</colorOpticalframeName>
+            <colorCameraInfoTopicName>color/camera_info</colorCameraInfoTopicName>
+            <!-- Infrared camera settings -->
+            <infraredUpdateRate>30</infraredUpdateRate>
+            <infrared1TopicName>infra1/image_rect_raw</infrared1TopicName>
+            <infrared2TopicName>infra2/image_rect_raw</infrared2TopicName>
+            <infrared1CameraInfoTopicName>infra1/camera_info</infrared1CameraInfoTopicName>
+            <infrared2CameraInfoTopicName>infra2/camera_info</infrared2CameraInfoTopicName>
+            <infrared1OpticalframeName>d400_arm_left_ir_optical_frame</infrared1OpticalframeName>
+            <infrared2OpticalframeName>d400_arm_right_ir_optical_frame</infrared2OpticalframeName>
+            <!-- Depth camera settings -->
+            <rangeMinDepth>0.2</rangeMinDepth>
+            <rangeMaxDepth>10.0</rangeMaxDepth>
+            <depthUpdateRate>30</depthUpdateRate>
+            <depthTopicName>aligned_depth_to_color/image_raw</depthTopicName>
+            <depthCameraInfoTopicName>aligned_depth_to_color/camera_info</depthCameraInfoTopicName>
+            <depthOpticalframeName>d400_arm_color_optical_frame</depthOpticalframeName>
+            <!-- Pointlcloud settings -->
+            <pointCloud>True</pointCloud>
+            <pointCloudTopicName>depth/color/points</pointCloudTopicName>
+            <pointCloudCutoff>0.5</pointCloudCutoff>
+        </plugin>
+    </gazebo>
     <joint name="manipulator_connector" type="fixed">
         <parent link="base_footprint" />
         <child link="ang_link0" />
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/config/ros_controllers.yaml b/src/omnibot_manipulator/omnibot_manipulator_moveit/config/ros_controllers.yaml
index 6385c9d..7a20126 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/config/ros_controllers.yaml
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/config/ros_controllers.yaml
@@ -1,6 +1,5 @@
 maipulator_group_controller:
-  type: effort_controllers/JointTrajectoryController
-
+  type: position_controllers/JointTrajectoryController
   joints:
     - ang_joint1
     - ang_joint2
@@ -33,14 +32,8 @@ maipulator_group_controller:
       d: 1
       i: 1
       i_clamp: 1
-
-  constraints:
-      goal_time: 0.6
-      stop_trajectory_duration: 0.1
-      stopped_velocity_tolerance: 0
 gripper_group_controller:
-  type: effort_controllers/JointTrajectoryController
-  
+  type: position_controllers/JointTrajectoryController
   joints:
     - ang_gripper
   gains:
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/config/sensors_3d.yaml b/src/omnibot_manipulator/omnibot_manipulator_moveit/config/sensors_3d.yaml
index 35db251..51010a3 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/config/sensors_3d.yaml
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/config/sensors_3d.yaml
@@ -1,27 +1,2 @@
-# Params for the octomap monitor
-
-# sensors:
-  # - sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
-  #   image_topic: /d400_arm/aligned_depth_to_color/image_raw
-  #   queue_size: 5
-  #   near_clipping_plane_distance: 0.01
-  #   far_clipping_plane_distance: 5.0
-  #   shadow_threshold: 0.2
-  #   padding_scale: 1.0
-  #   padding_offset: 0.05
-  #   max_update_rate: 10.0
-  #   filtered_cloud_topic: filtered_cloud
-  #   ns: d400_arm
-
-sensors: 
-  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
-    # point_cloud_topic: /rtabmap/cloud_map
-    # point_cloud_topic: /rtabmap/cloud_obstacles
-    # point_cloud_topic: /rtabmap/local_grid_obstacle
-    point_cloud_topic: /d400_arm/depth/color/points
-    max_range: 2.0
-    max_update_rate: 30.0
-    point_subsample: 1
-    padding_offset: 0.05
-    padding_scale: 1.0
-    filtered_cloud_topic: filtered_cloud
+sensors:
+  []
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/config/simple_moveit_controllers.yaml b/src/omnibot_manipulator/omnibot_manipulator_moveit/config/simple_moveit_controllers.yaml
index 652020b..ef7c87f 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/config/simple_moveit_controllers.yaml
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/config/simple_moveit_controllers.yaml
@@ -1,5 +1,5 @@
 controller_list:
-  - name: maipulator_group_controller
+  - name: /maipulator_group_controller
     action_ns: follow_joint_trajectory
     type: FollowJointTrajectory
     default: True
@@ -9,7 +9,7 @@ controller_list:
       - ang_joint3
       - ang_joint4
       - ang_joint5
-  - name: gripper_group_controller
+  - name: /gripper_group_controller
     action_ns: follow_joint_trajectory
     type: FollowJointTrajectory
     default: True
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/demo.launch b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/demo.launch
index c2b37e3..e89f7b7 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/demo.launch
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/demo.launch
@@ -15,7 +15,7 @@
   <arg name="load_robot_description" default="true"/>
 
   <!-- Choose controller manager: fake, simple, or ros_control -->
-  <arg name="moveit_controller_manager" default="ros_control" />
+  <arg name="moveit_controller_manager" default="simple" />
   <!-- Set execution mode for fake execution controllers -->
   <arg name="fake_execution_type" default="interpolate" />
 
@@ -24,13 +24,23 @@
   <arg name="use_rviz" default="true" />
 
   <!-- If needed, broadcast static tf for robot root -->
-  <include file="$(dirname)/ros_controllers.launch"/>
-  <param name="ros_control_namespace" value="omnibot_robot"/>
-	<param name="moveit_controller_manager" value="moveit_ros_control_interface::MoveItControllerManager" />
-    <!-- Spawn the Gazebo ROS controllers -->
 
+   <group if="$(eval arg('moveit_controller_manager') == 'simple')">
+    <!-- We do not have a real robot connected, so publish fake joint states via a
+    joint_state_publisher
+         MoveIt's fake controller's joint states are considered via the 'source_list' parameter -->
+    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
+      <rosparam param="source_list">[/omnibot_robot/manipulator_joint_states_correcter]</rosparam>
+    </node>
 
-    <!-- Given the published joint states, publish tf for the robot links -->
+    <node name="omnibot_robot_state_publisher" pkg="robot_state_publisher"
+      type="robot_state_publisher" ns="omnibot_robot">
+      <param name="publish_frequency" value="200" />
+      <param name="use_tf_static" value="false" />
+      <param name="rate" value="200" />
+
+    </node>
+  </group>
 
   <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
   <include file="$(dirname)/move_group.launch">
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/gazebo.launch b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/gazebo.launch
index 756248b..551f921 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/gazebo.launch
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/gazebo.launch
@@ -5,7 +5,7 @@
   <arg name="paused" default="false" doc="Start Gazebo paused"/>
   <arg name="world_name" default="worlds/empty.world" doc="Gazebo world file"/>
   <arg name="world_pose" default="-x 0 -y 0 -z 0 -R 0 -P 0 -Y 0" doc="Pose to spawn the robot at"/>
-  <arg name="initial_joint_positions" default="" doc="Initial joint configuration of the robot"/>
+  <arg name="initial_joint_positions" default=" -J ang_joint1 -3.14 -J ang_joint2 -0.3694 -J ang_joint3 -1.1698 -J ang_joint4 -0.0308 -J ang_joint5 0" doc="Initial joint configuration of the robot"/>
 
   <!-- Start Gazebo paused to allow the controllers to pickup the initial pose -->
   <include file="$(find gazebo_ros)/launch/empty_world.launch" pass_all_args="true">
@@ -23,12 +23,12 @@
     respawn="false" output="screen" />
 
   <!-- Load the controller parameters onto the parameter server -->
-  <rosparam file="$(find omnibot_manipulator_moveit)/config/gazebo_controllers.yaml" ns="omnibot_robot" />
+  <rosparam file="$(find omnibot_manipulator_moveit)/config/gazebo_controllers.yaml" />
   <include file="$(dirname)/ros_controllers.launch"/>
 
   <!-- Spawn the Gazebo ROS controllers -->
-  <node name="gazebo_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller" ns="omnibot_robot" />
+  <node name="gazebo_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller" />
 
   <!-- Given the published joint states, publish tf for the robot links -->
-  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" ns="omnibot_robot" />
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
 </launch>
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/joystick_control.launch b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/joystick_control.launch
index 4278362..9411f6e 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/joystick_control.launch
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/joystick_control.launch
@@ -4,6 +4,12 @@
   <arg name="dev" default="/dev/input/js0" />
 
   <!-- Launch joy node -->
+  <node pkg="joy" type="joy_node" name="joy">
+    <param name="dev" value="$(arg dev)" /> <!-- Customize this to match the location your joystick is plugged in on-->
+    <param name="deadzone" value="0.2" />
+    <param name="autorepeat_rate" value="40" />
+    <param name="coalesce_interval" value="0.025" />
+  </node>
 
   <!-- Launch python interface -->
   <node pkg="moveit_ros_visualization" type="moveit_joy.py" output="screen" name="moveit_joy"/>
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/move_group.launch b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/move_group.launch
index a93907e..1fcd7a1 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/move_group.launch
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/move_group.launch
@@ -100,8 +100,6 @@
     <param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
     <param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
     <param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />
-    <remap from="joint_states" to="/omnibot_robot/joint_states"/>
-    <remap from="/controller_manager/list_controllers" to="/omnibot_robot/controller_manager/list_controller"/>
   </node>
 
 </launch>
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/omnibot_robot_moveit_sensor_manager.launch.xml b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/omnibot_robot_moveit_sensor_manager.launch.xml
index 018e42d..5d02698 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/omnibot_robot_moveit_sensor_manager.launch.xml
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/omnibot_robot_moveit_sensor_manager.launch.xml
@@ -1,4 +1,3 @@
 <launch>
-    <rosparam command="load" file="$(find omnibot_manipulator_moveit)/config/sensors_3d.yaml"/>
-    
+
 </launch>
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/ros_control_moveit_controller_manager.launch.xml b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/ros_control_moveit_controller_manager.launch.xml
index dc8f282..3b92a2b 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/ros_control_moveit_controller_manager.launch.xml
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/ros_control_moveit_controller_manager.launch.xml
@@ -1,6 +1,7 @@
 <launch>
 	<!-- Define MoveIt controller manager plugin -->
-	<param name="ros_control_namespace" value="omnibot_robot"/>
+	<param name="ros_control_namespace" value="/omnibot_robot/"/>
 	
-	<param name="moveit_controller_manager" value="moveit_ros_control_interface::MoveItControllerManager" />
+	<param name="moveit_controller_manager" value="moveit_ros_control_interface::MoveItControllerManager"/>
+
 </launch>
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/sensor_manager.launch.xml b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/sensor_manager.launch.xml
index 3315168..63f5e6f 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/sensor_manager.launch.xml
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/sensor_manager.launch.xml
@@ -4,14 +4,14 @@
 
   <!-- Params for 3D sensors config -->
   <rosparam command="load" file="$(find omnibot_manipulator_moveit)/config/sensors_3d.yaml" />
-  <!-- Params for the octomap monitor -->
-  <param name="octomap_frame" type="string" value="d400_arm" />
-  <param name="octomap_resolution" type="double" value="0.1" /> <!-- in meters -->
-  <param name="max_range" type="double" value="2.5" />
 
+  <!-- Params for the octomap monitor -->
+  <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
+  <param name="octomap_resolution" type="double" value="0.025" />
+  <param name="max_range" type="double" value="5.0" />
 
   <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
   <arg name="moveit_sensor_manager" default="omnibot_robot" />
   <include file="$(dirname)/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />
 
-</launch>
\ No newline at end of file
+</launch>
diff --git a/src/omnibot_platform/omnibot_platform_navigation/launch/rtabmap.launch b/src/omnibot_platform/omnibot_platform_navigation/launch/rtabmap.launch
index bcb99fc..008752c 100644
--- a/src/omnibot_platform/omnibot_platform_navigation/launch/rtabmap.launch
+++ b/src/omnibot_platform/omnibot_platform_navigation/launch/rtabmap.launch
@@ -30,7 +30,7 @@
       
         <!-- RTAB-Map's parameters -->
         <!-- ################################################### -->
-        <param name="Rtabmap/TimeThr"                           type="string"   value="700" />
+        <param name="Rtabmap/TimeThr"                           type="string"   value="0" />
         <param name="Rtabmap/DetectionRate"                     type="string"   value="10" />
         <param name="Rtabmap/ImageBufferSize"                   type="string"   value="0" />
         <param name="Rtabmap/StartNewMapOnLoopClosure"          type="string"   value="true"/> 
@@ -43,7 +43,6 @@
         <param name="RGBD/OptimizeMaxError"                     type="double"   value="0"/> <!-- Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled-->
         <param name="RGBD/ProximityPathMaxNeighbors"            type="string"   value="0" />
         <param name="RGBD/CreateOccupancyGrid"                  type="string"   value="true" /> <!-- The proximity detection using only laser scans is disabled-->
-        <param name="RGBD/StartAtOrigin"                        type="string"   value="false"/>
         <param name="RGBD/NeighborLinkRefining"                 type="string"   value="true"/>  <!-- Do odometry correction with consecutive laser scans -->
         <param name="RGBD/ProximityBySpace"                     type="string"   value="true" /> <!-- Local loop closure detection (using estimated position) with locations in WM -->
         <param name="RGBD/ProximityByTime"                      type="string"   value="true"/> <!-- Local loop closure detection with locations in STM -->
@@ -64,7 +63,7 @@
         <param name="Reg/Strategy"                              type="string"   value="2" /><!-- Loop closure transformation: 0=Visual, 1=ICP, 2=Visual+ICP -->
         <!-- ################################################### -->
         <param name="Mem/RehearsalSimilarity"                   type="string"   value="0.30"/>  <!--  Rehearsal similarity.-->
-        <param name="Mem/DepthAsMask"                           type="bool"     value="false"/> <!-- Use depth image as mask when extracting features for vocabulary-->
+        <!-- <param name="Mem/DepthAsMask"                           type="bool"     value="false"/> Use depth image as mask when extracting features for vocabulary -->
         <param name="Mem/IncrementalMemory"                     type="string"   value="false"     if="$(arg localization)"/>
         <param name="Mem/IncrementalMemory"                     type="string"   value="true"     unless="$(arg localization)"/>
         <param name="Mem/InitWMWithAllNodes"                    type="string"   value="$(arg localization)"/>
@@ -72,13 +71,12 @@
         <!-- ################################################### -->
         <param name="Icp/CorrespondenceRatio"      type="string" value="0.3"/>
         <!-- ################################################### -->
-        <param name="Vis/MinInliers"                            type="string"   value="10" /><!-- 3D visual words minimum inliers to accept loop closure -->
+        <param name="Vis/MinInliers"                            type="string"   value="6" /><!-- 3D visual words minimum inliers to accept loop closure -->
         <param name="Vis/InlierDistance"                        type="string"   value="0.1" /> <!-- 3D visual words correspondence distance -->
         <!-- ################################################### -->
-        <param name="Optimizer/Slam2D"                          type="string"   value="true"/>
         <!-- ################################################### -->
         <param name="Odom/Strategy"                             type="string"   value="0"/> <!-- 0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D-->
-        <param name="Odom/Holonomic"		                    type="string"   value="true"/> <!-- If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).-->
+        <!-- <param name="Odom/Holonomic"		                    ty/pe="string"   value="true"/> If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)). -->
         <param name="Odom/FilteringStrategy"                    type="string"   value="0"/> <!-- 0=No filtering 1=Kalman filtering 2=Particle filtering. This filter is used to smooth the odometry output.-->
 <!-- ################################################################################################################################################################# -->
         <!-- REMAPS -->

From bc1d941a079039e797c97a8b9d7953f81adddb8f Mon Sep 17 00:00:00 2001
From: IvanSnap360 <ivpetr2003@gmail.com>
Date: Fri, 9 Jun 2023 20:13:33 +0300
Subject: [PATCH 06/13] add sim switcher

---
 src/omnibot_bringup/launch/omnibot_sim.launch |  3 +-
 .../launch/load_description.launch            |  1 -
 .../CMakeLists.txt                            |  4 +-
 .../config/controllers.yaml                   |  6 --
 .../config/ros_controllers.yaml               |  4 +-
 .../launch/demo.launch                        | 62 +++++++++++--------
 .../launch/move_group.launch                  |  5 +-
 ...ntrol_moveit_controller_manager.launch.xml |  5 +-
 8 files changed, 49 insertions(+), 41 deletions(-)
 delete mode 100644 src/omnibot_manipulator/omnibot_manipulator_moveit/config/controllers.yaml

diff --git a/src/omnibot_bringup/launch/omnibot_sim.launch b/src/omnibot_bringup/launch/omnibot_sim.launch
index 29cf9d1..e67442b 100644
--- a/src/omnibot_bringup/launch/omnibot_sim.launch
+++ b/src/omnibot_bringup/launch/omnibot_sim.launch
@@ -38,6 +38,7 @@
 
         <include file="$(find omnibot_manipulator_moveit)/launch/demo.launch">
             <arg name="use_rviz" value="false"/>
+            <arg name="sim" value="true"/>
         </include>
     </group>
 
@@ -65,8 +66,6 @@
         <arg name="initial_pose"            value=""/>     
     </include>
 
-
-
 <!-- START ROS WEB BRIDGE -->
     <include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch"/>
 </launch>
\ No newline at end of file
diff --git a/src/omnibot_description/launch/load_description.launch b/src/omnibot_description/launch/load_description.launch
index 6600b78..75edd48 100644
--- a/src/omnibot_description/launch/load_description.launch
+++ b/src/omnibot_description/launch/load_description.launch
@@ -32,7 +32,6 @@
         <param name="publish_frequency" value="200" />
         <param name="use_tf_static" value="false" />
         <param name="rate" value="200" />
-
     </node>
 
 </launch>
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/CMakeLists.txt b/src/omnibot_manipulator/omnibot_manipulator_control/CMakeLists.txt
index 9c9a6e4..5be0435 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_control/CMakeLists.txt
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/CMakeLists.txt
@@ -19,6 +19,8 @@ find_package(catkin REQUIRED COMPONENTS
   moveit_core
   moveit_ros_planning_interface
   moveit_visual_tools
+  actionlib_msgs
+  actionlib
 )
 
 ## System dependencies are found with CMake's conventions
@@ -138,7 +140,7 @@ generate_messages(
 catkin_package(
  INCLUDE_DIRS include
  LIBRARIES omnibot_manipulator_control_server_lib 
- CATKIN_DEPENDS roscpp rospy std_msgs dynamixel_workbench_msgs trajectory_msgs dynamixel_workbench_toolbox cmake_modules
+ CATKIN_DEPENDS roscpp rospy std_msgs dynamixel_workbench_msgs trajectory_msgs dynamixel_workbench_toolbox cmake_modules actionlib_msgs actionlib
 #  DEPENDS system_lib
 )
 
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/config/controllers.yaml b/src/omnibot_manipulator/omnibot_manipulator_moveit/config/controllers.yaml
deleted file mode 100644
index 694ab23..0000000
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/config/controllers.yaml
+++ /dev/null
@@ -1,6 +0,0 @@
-controller_list:
-  - name: maipulator_arm_contoller
-    action_ns: joint_trajectory_controller
-    type: FollowJointTrajectory
-    default: true
-    joints: [ang_joint1, ang_joint2, ang_joint3, ang_joint4, ang_joint5]
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/config/ros_controllers.yaml b/src/omnibot_manipulator/omnibot_manipulator_moveit/config/ros_controllers.yaml
index 7a20126..85ee858 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/config/ros_controllers.yaml
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/config/ros_controllers.yaml
@@ -1,5 +1,5 @@
 maipulator_group_controller:
-  type: position_controllers/JointTrajectoryController
+  type: effort_controllers/JointTrajectoryController
   joints:
     - ang_joint1
     - ang_joint2
@@ -33,7 +33,7 @@ maipulator_group_controller:
       i: 1
       i_clamp: 1
 gripper_group_controller:
-  type: position_controllers/JointTrajectoryController
+  type: effort_controllers/JointTrajectoryController
   joints:
     - ang_gripper
   gains:
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/demo.launch b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/demo.launch
index e89f7b7..0fc39d8 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/demo.launch
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/demo.launch
@@ -10,58 +10,70 @@
 
   <!-- By default, we are not in debug mode -->
   <arg name="debug" default="false" />
-
+  <arg name="sim" default="false" />
   <!-- By default, we will load or override the robot_description -->
-  <arg name="load_robot_description" default="true"/>
+  <arg name="load_robot_description" default="true" />
 
   <!-- Choose controller manager: fake, simple, or ros_control -->
-  <arg name="moveit_controller_manager" default="simple" />
+  <arg name="moveit_controller_manager" default="ros_control" if="$(arg sim)" />
+  <arg name="moveit_controller_manager" default="simple" unless="$(arg sim)" />
   <!-- Set execution mode for fake execution controllers -->
   <arg name="fake_execution_type" default="interpolate" />
 
+
   <!-- By default, hide joint_state_publisher's GUI in 'fake' controller_manager mode -->
   <arg name="use_gui" default="false" />
   <arg name="use_rviz" default="true" />
 
   <!-- If needed, broadcast static tf for robot root -->
+  <include file="$(dirname)/ros_controllers.launch" />
+  <param name="ros_control_namespace" value="omnibot_robot" />
+  <param name="moveit_controller_manager"
+    value="moveit_ros_control_interface::MoveItControllerManager" />
+  <!-- Spawn the Gazebo ROS controllers -->
 
-   <group if="$(eval arg('moveit_controller_manager') == 'simple')">
-    <!-- We do not have a real robot connected, so publish fake joint states via a
+  <!-- If needed, broadcast static tf for robot root -->
+  <group unless="$(arg sim)">
+    <group if="$(eval arg('moveit_controller_manager') == 'simple')">
+      <!-- We do not have a real robot connected, so publish fake joint states via a
     joint_state_publisher
          MoveIt's fake controller's joint states are considered via the 'source_list' parameter -->
-    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
-      <rosparam param="source_list">[/omnibot_robot/manipulator_joint_states_correcter]</rosparam>
-    </node>
+      <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
+        <rosparam param="source_list">[/omnibot_robot/manipulator_joint_states_correcter]</rosparam>
+      </node>
 
-    <node name="omnibot_robot_state_publisher" pkg="robot_state_publisher"
-      type="robot_state_publisher" ns="omnibot_robot">
-      <param name="publish_frequency" value="200" />
-      <param name="use_tf_static" value="false" />
-      <param name="rate" value="200" />
+      <node name="omnibot_robot_state_publisher" pkg="robot_state_publisher"
+        type="robot_state_publisher" ns="omnibot_robot">
+        <param name="publish_frequency" value="200" />
+        <param name="use_tf_static" value="false" />
+        <param name="rate" value="200" />
 
-    </node>
+      </node>
+    </group>
   </group>
 
-  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
+  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers
+  configured by default) -->
   <include file="$(dirname)/move_group.launch">
-    <arg name="allow_trajectory_execution" value="true"/>
+    <arg name="allow_trajectory_execution" value="true" />
     <arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />
-    <arg name="fake_execution_type" value="$(arg fake_execution_type)"/>
-    <arg name="info" value="true"/>
-    <arg name="debug" value="$(arg debug)"/>
-    <arg name="pipeline" value="$(arg pipeline)"/>
-    <arg name="load_robot_description" value="$(arg load_robot_description)"/>
+    <arg name="fake_execution_type" value="$(arg fake_execution_type)" />
+    <arg name="info" value="true" />
+    <arg name="debug" value="$(arg debug)" />
+    <arg name="pipeline" value="$(arg pipeline)" />
+    <arg name="load_robot_description" value="$(arg load_robot_description)" />
+    <arg name="sim" value="$(arg sim)" />
   </include>
 
   <!-- Run Rviz and load the default config to see the state of the move_group node -->
   <include file="$(dirname)/moveit_rviz.launch" if="$(arg use_rviz)">
-    <arg name="rviz_config" value="$(dirname)/moveit.rviz"/>
-    <arg name="debug" value="$(arg debug)"/>
+    <arg name="rviz_config" value="$(dirname)/moveit.rviz" />
+    <arg name="debug" value="$(arg debug)" />
   </include>
 
   <!-- If database loading was enabled, start mongodb as well -->
   <include file="$(dirname)/default_warehouse_db.launch" if="$(arg db)">
-    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
+    <arg name="moveit_warehouse_database_path" value="$(arg db_path)" />
   </include>
 
-</launch>
+</launch>
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/move_group.launch b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/move_group.launch
index 1fcd7a1..50bf34e 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/move_group.launch
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/move_group.launch
@@ -18,7 +18,8 @@
   <arg name="fake_execution_type" default="interpolate"/>
   <arg name="max_safe_path_cost" default="1"/>
   <arg name="publish_monitored_planning_scene" default="true"/>
-
+  <arg name="sim" default="false"/>
+  
   <arg name="capabilities" default=""/>
   <arg name="disable_capabilities" default=""/>
   <!-- load these non-default MoveGroup capabilities (space seperated) -->
@@ -100,6 +101,8 @@
     <param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
     <param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
     <param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />
+    <remap from="joint_states" to="/omnibot_robot/joint_states" if="$(arg sim)"/>
+    <remap from="/controller_manager/list_controllers" to="/omnibot_robot/controller_manager/list_controller" if="$(arg sim)"/>
   </node>
 
 </launch>
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/ros_control_moveit_controller_manager.launch.xml b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/ros_control_moveit_controller_manager.launch.xml
index 3b92a2b..53fd135 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/ros_control_moveit_controller_manager.launch.xml
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/ros_control_moveit_controller_manager.launch.xml
@@ -1,7 +1,6 @@
 <launch>
 	<!-- Define MoveIt controller manager plugin -->
-	<param name="ros_control_namespace" value="/omnibot_robot/"/>
-	
-	<param name="moveit_controller_manager" value="moveit_ros_control_interface::MoveItControllerManager"/>
+	<param name="ros_control_namespace" value="omnibot_robot"/>
 
+	<param name="moveit_controller_manager" value="moveit_ros_control_interface::MoveItControllerManager" />
 </launch>

From d9baf562248dab478a4965a3fc9e732b566236e1 Mon Sep 17 00:00:00 2001
From: IvanSnap360 <ivpetr2003@gmail.com>
Date: Fri, 9 Jun 2023 21:29:06 +0300
Subject: [PATCH 07/13] copy trajectory retraslator to server (manip)

---
 .../CMakeLists.txt                            |  8 ++--
 ...bot_manipulator_control_server_config.yaml | 10 ++++
 .../action_server_lib/action_server_lib.h     | 19 ++++++++
 .../omnibot_manipulator_control_server_lib.h  | 20 +++++++-
 .../omnibot_manipulator_control/package.xml   |  7 ++-
 .../action_server_lib/action_server_lib.cpp   | 39 +++++++++++++++
 ...omnibot_manipulator_control_server_lib.cpp | 48 +++++++++++++++----
 7 files changed, 135 insertions(+), 16 deletions(-)
 create mode 100644 src/omnibot_manipulator/omnibot_manipulator_control/include/action_server_lib/action_server_lib.h
 create mode 100644 src/omnibot_manipulator/omnibot_manipulator_control/src/action_server_lib/action_server_lib.cpp

diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/CMakeLists.txt b/src/omnibot_manipulator/omnibot_manipulator_control/CMakeLists.txt
index 5be0435..8783524 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_control/CMakeLists.txt
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/CMakeLists.txt
@@ -139,7 +139,7 @@ generate_messages(
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
  INCLUDE_DIRS include
- LIBRARIES omnibot_manipulator_control_server_lib 
+ LIBRARIES omnibot_manipulator_control_server_lib action_server_lib
  CATKIN_DEPENDS roscpp rospy std_msgs dynamixel_workbench_msgs trajectory_msgs dynamixel_workbench_toolbox cmake_modules actionlib_msgs actionlib
 #  DEPENDS system_lib
 )
@@ -159,7 +159,8 @@ include_directories(
 ## Declare a C++ library
 add_library(${PROJECT_NAME}
   src/${PROJECT_NAME}_server_lib/omnibot_manipulator_control_server_lib.cpp
-)
+  src/action_server_lib/action_server_lib.cpp
+  )
 
 ## Add cmake target dependencies of the library
 ## as an example, code may need to be generated before libraries
@@ -171,7 +172,8 @@ add_library(${PROJECT_NAME}
 ## The recommended prefix ensures that target names across packages don't collide
 add_executable(${PROJECT_NAME}_server_node
   src/omnibot_manipulator_control_server_node.cpp 
-  src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp)
+  src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp
+  src/action_server_lib/action_server_lib.cpp)
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
 ## target back to the shorter version for ease of user use
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/config/omnibot_manipulator_control_server_config.yaml b/src/omnibot_manipulator/omnibot_manipulator_control/config/omnibot_manipulator_control_server_config.yaml
index 344d850..7bb3261 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_control/config/omnibot_manipulator_control_server_config.yaml
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/config/omnibot_manipulator_control_server_config.yaml
@@ -18,3 +18,13 @@ services:
   gripper_cmd_service:
     name: gripper_cmd
     saved_poses: [{ name: "open", val: 0.025 }, { name: "close", val: 0.01 }]
+
+
+hardware:
+  default_state: false # on / off on startup
+  trajectoryFollowersEnableServiceName: trajectory_followers_enable
+  input:
+    manipulator_action_server_name: /maipulator_group_controller/follow_joint_trajectory
+    gripper_action_server_name:     /gripper_group_controller/follow_joint_trajectory
+  output:
+    actuators_trajectory_topic: /dynamixel_contoller/joint_trajectory
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/include/action_server_lib/action_server_lib.h b/src/omnibot_manipulator/omnibot_manipulator_control/include/action_server_lib/action_server_lib.h
new file mode 100644
index 0000000..186e24a
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/include/action_server_lib/action_server_lib.h
@@ -0,0 +1,19 @@
+#include <ros/ros.h>
+#include <actionlib/server/simple_action_server.h>
+#include <control_msgs/FollowJointTrajectoryAction.h>
+#include <trajectory_msgs/JointTrajectory.h>
+
+class RobotTrajectoryFollower
+{
+private:
+    ros::NodeHandle *_nh;
+    ros::Publisher _trajectory_publisher;
+    actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> *_as;
+    bool _state;
+public:
+    RobotTrajectoryFollower(ros::NodeHandle *_nh,std::string action_server_name,std::string traj_pub_name);
+    ~RobotTrajectoryFollower();
+    void setState(bool state);
+    void goalCB();
+    void preemptCB();
+};
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/include/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.h b/src/omnibot_manipulator/omnibot_manipulator_control/include/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.h
index 1f112be..32bec28 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_control/include/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.h
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/include/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.h
@@ -11,6 +11,10 @@
 #include <omnibot_manipulator_control/manipulator_cmd.h>
 #include <omnibot_manipulator_control/gripper_cmd.h>
 #include <std_msgs/Empty.h>
+#include "action_server_lib/action_server_lib.h"
+#include <control_msgs/FollowJointTrajectoryAction.h>
+#include <trajectory_msgs/JointTrajectory.h>
+#include <std_srvs/SetBool.h>
 
 #define degreesToRadians(degrees) degrees *M_PI / 180
 
@@ -39,6 +43,17 @@ class OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB
     ros::ServiceServer _arm_cmd_service_server;
     ros::ServiceServer _gripper_cmd_service_server;
 
+    RobotTrajectoryFollower *_arm_trajectory_follower;
+    RobotTrajectoryFollower *_gripper_trajectory_follower;
+    bool _trajectory_followers_state = false;
+    std::string _trajectory_followers_enable_service_name;
+    std::string _manipulator_action_server_name;
+    std::string _gripper_action_server_name;
+    std::string _actuators_trajectory_topic_name;
+    ros::ServiceServer _trajectory_followers_enable_service;
+    bool _trajectory_followers_enable_service_cb_f(std_srvs::SetBool::Request &req,
+                                                   std_srvs::SetBool::Response &res);
+
     bool ManipMoveByJointValues(std::vector<double> joint_values);
     bool ManipMoveByPosition(geometry_msgs::Pose new_pose);
     bool ManipMoveBySavedPosition(std::string pose_name);
@@ -51,10 +66,11 @@ class OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB
                                       omnibot_manipulator_control::manipulator_cmd::Response &res);
     bool _grp_cmd_service_server_cb_f(omnibot_manipulator_control::gripper_cmd::Request &req,
                                       omnibot_manipulator_control::gripper_cmd::Response &res);
-    
+
     ros::Publisher _rviz_goal_state_updater_pub;
-    
+
     void updateGoalState();
+
 public:
     OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB(ros::NodeHandle *nh, std::string config_path);
     void init();
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/package.xml b/src/omnibot_manipulator/omnibot_manipulator_control/package.xml
index 9070b09..3bd7404 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_control/package.xml
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/package.xml
@@ -76,7 +76,12 @@
   <build_export_depend>moveit_core</build_export_depend>
   <build_export_depend>moveit_ros_planning_interface</build_export_depend>
   <build_export_depend>moveit_visual_tools</build_export_depend>
-
+  <exec_depend>actionlib_msgs</exec_depend>
+  <build_export_depend>actionlib_msgs</build_export_depend>
+  <build_depend>actionlib_msgs</build_depend>
+  <build_depend>actionlib</build_depend>
+  <build_export_depend>actionlib</build_export_depend>
+  <exec_depend>actionlib</exec_depend>
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <!-- Other tools can request additional information be placed here -->
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/src/action_server_lib/action_server_lib.cpp b/src/omnibot_manipulator/omnibot_manipulator_control/src/action_server_lib/action_server_lib.cpp
new file mode 100644
index 0000000..3e54b21
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/src/action_server_lib/action_server_lib.cpp
@@ -0,0 +1,39 @@
+#include "action_server_lib/action_server_lib.h"
+
+RobotTrajectoryFollower::RobotTrajectoryFollower(ros::NodeHandle *_nh, std::string action_server_name, std::string traj_pub_name)
+{
+
+    _as = new actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction>(action_server_name, true);
+    // Register callback functions:
+    _as->registerGoalCallback(boost::bind(&RobotTrajectoryFollower::goalCB, this));
+    _as->registerPreemptCallback(boost::bind(&RobotTrajectoryFollower::preemptCB, this));
+    // _as->start();
+
+    _trajectory_publisher = _nh->advertise<trajectory_msgs::JointTrajectory>(traj_pub_name, 10);
+}
+
+RobotTrajectoryFollower::~RobotTrajectoryFollower() // Destructor
+{
+}
+
+void RobotTrajectoryFollower::goalCB()
+{
+    std::cout << "goalCB!!!" << std::endl;
+    if (_state)
+    {
+        _trajectory_publisher.publish(_as->acceptNewGoal()->trajectory);
+    }
+}
+
+void RobotTrajectoryFollower::preemptCB()
+{
+    if (_state)
+    {
+        _as->setPreempted();
+    }
+}
+
+void RobotTrajectoryFollower::setState(bool state)
+{
+    _state = state;
+}
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp b/src/omnibot_manipulator/omnibot_manipulator_control/src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp
index fd4b29b..8c2da6f 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_control/src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp
@@ -1,14 +1,11 @@
 #include "omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.h"
 
-
-
 void OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::updateGoalState()
 {
     std_msgs::Empty _rviz_goal_state_updater_msg;
     _rviz_goal_state_updater_pub.publish(_rviz_goal_state_updater_msg);
 }
 
-
 OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB(ros::NodeHandle *nh, std::string config_path)
 {
     _cfg = YAML::LoadFile(config_path);
@@ -72,8 +69,31 @@ OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB(r
         }
     }
     _gripper_cmd_service_name = _cfg["services"]["gripper_cmd_service"]["name"].as<std::string>();
+    // ##################################################################### //
+    // ######################## READ HARDWARE CONFIG ####################### //
+    // ##################################################################### //
+    _trajectory_followers_state = _cfg["hardware"]["default_state"].as<bool>();
+    _trajectory_followers_enable_service_name = _cfg["hardware"]["trajectoryFollowersEnableServiceName"].as<std::string>();
+    _manipulator_action_server_name = _cfg["hardware"]["input"]["manipulator_action_server_name"].as<std::string>();
+    _gripper_action_server_name = _cfg["hardware"]["input"]["gripper_action_server_name"].as<std::string>();
+    _actuators_trajectory_topic_name = _cfg["hardware"]["output"]["actuators_trajectory_topic"].as<std::string>();
+
+    _arm_trajectory_follower = new RobotTrajectoryFollower(_nh,
+                                                           _manipulator_action_server_name,
+                                                           _actuators_trajectory_topic_name);
+
+    _gripper_trajectory_follower = new RobotTrajectoryFollower(_nh,
+                                                               _gripper_action_server_name,
+                                                               _actuators_trajectory_topic_name);
+
+    _trajectory_followers_enable_service = _nh->advertiseService(
+        _trajectory_followers_enable_service_name, 
+        &OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::_trajectory_followers_enable_service_cb_f,
+        this);
+    
+
 
-    _rviz_goal_state_updater_pub = _nh->advertise<std_msgs::Empty>("/rviz/moveit/update_goal_state",10);
+    _rviz_goal_state_updater_pub = _nh->advertise<std_msgs::Empty>("/rviz/moveit/update_goal_state", 10);
 }
 
 void OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::init()
@@ -111,7 +131,6 @@ void OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::init()
     ROS_INFO("Initilazing gripper move group interface      DONE");
 }
 
-
 // ##################################################################### //
 // ####################### MANIPULATOR FUNCTIONS ####################### //
 // ##################################################################### //
@@ -162,7 +181,7 @@ bool OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::ManipMoveByJointValues(std::vector<
         return false;
     }
     _move_group_interface_arm->setJointValueTarget(joint_values);
-    
+
     ROS_INFO("Planning for joints positions [%.2lf %.2lf %.2lf %.2lf %.2lf]",
              joint_values[0], joint_values[1], joint_values[2], joint_values[3], joint_values[4]);
     updateGoalState();
@@ -252,10 +271,9 @@ bool OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::GripperMoveBySavedPosition(std::str
     }
 }
 
-
 // ##################################################################### //
 // #################### SERVISES CALLBACK FUNCTIONS #################### //
-// ##################################################################### // 
+// ##################################################################### //
 
 bool OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::_grp_cmd_service_server_cb_f(omnibot_manipulator_control::gripper_cmd::Request &req,
                                                                           omnibot_manipulator_control::gripper_cmd::Response &res)
@@ -298,8 +316,6 @@ bool OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::_grp_cmd_service_server_cb_f(omnibo
     return true;
 }
 
-
-
 bool OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::_arm_cmd_service_server_cb_f(
     omnibot_manipulator_control::manipulator_cmd::Request &req,
     omnibot_manipulator_control::manipulator_cmd::Response &res)
@@ -379,6 +395,18 @@ bool OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::_arm_cmd_service_server_cb_f(
     return true;
 }
 
+
+bool OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::_trajectory_followers_enable_service_cb_f(std_srvs::SetBool::Request &req,
+                                                   std_srvs::SetBool::Response &res)
+{
+    _arm_trajectory_follower->setState((bool)req.data);
+    _gripper_trajectory_follower->setState((bool)req.data);
+    res.message = "Trajectory followers is " + req.data ? "ON" : "OFF";
+    res.success = true;
+    return true;
+}
+
+
 OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::~OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB()
 {
 }
\ No newline at end of file

From 9cca7344fb677f7669425f8d91d2961ace7b5bca Mon Sep 17 00:00:00 2001
From: IvanSnap360 <ivpetr2003@gmail.com>
Date: Sat, 10 Jun 2023 08:58:42 +0300
Subject: [PATCH 08/13] add omnibot_core startup actions

---
 src/omnibot_bringup/launch/omnibot_sim.launch |  3 ++
 src/omnibot_core/CMakeLists.txt               | 12 ++++-
 src/omnibot_core/config/config.yaml           |  9 ++--
 .../omnibot_core_lib/omnibot_core_lib.h       | 11 ++++-
 .../state_machine_lib/state_machine_lib.h     |  5 +-
 src/omnibot_core/package.xml                  |  4 ++
 .../src/omnibot_core_lib/omnibot_core_lib.cpp | 47 ++++++++++++++++++-
 .../state_machine_lib/state_machine_lib.cpp   |  2 +-
 .../CMakeLists.txt                            | 34 ++++++++------
 ...omnibot_manipulator_control_server_lib.cpp | 13 +++--
 10 files changed, 103 insertions(+), 37 deletions(-)

diff --git a/src/omnibot_bringup/launch/omnibot_sim.launch b/src/omnibot_bringup/launch/omnibot_sim.launch
index e67442b..eb9e565 100644
--- a/src/omnibot_bringup/launch/omnibot_sim.launch
+++ b/src/omnibot_bringup/launch/omnibot_sim.launch
@@ -42,6 +42,9 @@
         </include>
     </group>
 
+
+    <include file="$(find omnibot_core)/launch/omnibot_core.launch"/>
+
     <!--  -->
     <group if="$(arg rviz)">
         <include file="$(find omnibot_bringup)/launch/rviz.launch" />
diff --git a/src/omnibot_core/CMakeLists.txt b/src/omnibot_core/CMakeLists.txt
index 953c8d5..afcda07 100644
--- a/src/omnibot_core/CMakeLists.txt
+++ b/src/omnibot_core/CMakeLists.txt
@@ -14,8 +14,12 @@ find_package(catkin REQUIRED COMPONENTS
   moveit_core
   moveit_ros_planning_interface
   moveit_visual_tools
+  
 )
 
+find_package(omnibot_manipulator_control REQUIRED COMPONENTS ...)
+include_directories(${omnibot_manipulator_control_INCLUDE_DIRS})
+
 ## System dependencies are found with CMake's conventions
 find_package(Boost REQUIRED COMPONENTS system)
 
@@ -153,12 +157,16 @@ add_library(${PROJECT_NAME}
 ## Add cmake target dependencies of the library
 ## as an example, code may need to be generated before libraries
 ## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
 
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
-add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}_node.cpp src/${PROJECT_NAME}_lib/${PROJECT_NAME}_lib.cpp src/state_machine_lib/state_machine_lib.cpp )
+add_executable(${PROJECT_NAME}_node 
+                src/${PROJECT_NAME}_node.cpp 
+                src/${PROJECT_NAME}_lib/${PROJECT_NAME}_lib.cpp   
+                src/state_machine_lib/state_machine_lib.cpp )
 
 
 ## Rename C++ executable without prefix
diff --git a/src/omnibot_core/config/config.yaml b/src/omnibot_core/config/config.yaml
index 3a8610d..bde20db 100644
--- a/src/omnibot_core/config/config.yaml
+++ b/src/omnibot_core/config/config.yaml
@@ -1,7 +1,4 @@
-manipulator_cmd:
-  service_name:
-  startup_command: {type: 2, value: home}
+manipulator_on: true
+manipulator_cmd_service_name: manipulator_cmd_service
+gripper_cmd_service_name: gripper_cmd
 
-gripper_cmd:
-  service_name:
-  startup_command: {type: 2, value: open}
\ No newline at end of file
diff --git a/src/omnibot_core/include/omnibot_core_lib/omnibot_core_lib.h b/src/omnibot_core/include/omnibot_core_lib/omnibot_core_lib.h
index ecf9116..94cb7fd 100644
--- a/src/omnibot_core/include/omnibot_core_lib/omnibot_core_lib.h
+++ b/src/omnibot_core/include/omnibot_core_lib/omnibot_core_lib.h
@@ -1,12 +1,21 @@
 #ifndef __OMNIBOT_CORE_LIB_H__
 #define __OMNIBOT_CORE_LIB_H__
 #include <ros/ros.h>
+#include <omnibot_manipulator_control/manipulator_cmd.h>
+#include <omnibot_manipulator_control/gripper_cmd.h>
 #include <yaml-cpp/yaml.h>
 
 class OMNIBOT_CORE_LIB
 {
 private:
-    YAML::Node _config;
+    YAML::Node _cfg;
+    ros::NodeHandle *_nh;
+
+    std::string _manipulator_service_name;
+    ros::ServiceClient _manipulator_service_client;
+
+    std::string _gripper_service_name;
+    ros::ServiceClient _gripper_service_client;
 
 public:
     OMNIBOT_CORE_LIB(ros::NodeHandle *nh, std::string config_path);
diff --git a/src/omnibot_core/include/state_machine_lib/state_machine_lib.h b/src/omnibot_core/include/state_machine_lib/state_machine_lib.h
index 652af5e..ccd0a99 100644
--- a/src/omnibot_core/include/state_machine_lib/state_machine_lib.h
+++ b/src/omnibot_core/include/state_machine_lib/state_machine_lib.h
@@ -6,11 +6,10 @@ class STATE_MACHINE_LIB
 {
 private:
     ros::NodeHandle *_nh;
+
 public:
-    STATE_MACHINE_LIB();
+    STATE_MACHINE_LIB(ros::NodeHandle *nh);
     ~STATE_MACHINE_LIB();
 };
 
-
-
 #endif // __STATE_MACHINE_LIB_H__
\ No newline at end of file
diff --git a/src/omnibot_core/package.xml b/src/omnibot_core/package.xml
index b7c794e..56d0eda 100644
--- a/src/omnibot_core/package.xml
+++ b/src/omnibot_core/package.xml
@@ -52,21 +52,25 @@
   <build_depend>roscpp</build_depend>
   <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>omnibot_manipulator_control</build_depend>
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>rospy</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
   <build_export_depend>moveit_core</build_export_depend>
   <build_export_depend>moveit_ros_planning_interface</build_export_depend>
   <build_export_depend>moveit_visual_tools</build_export_depend>
+  <build_export_depend>omnibot_manipulator_control</build_export_depend>
   <exec_depend>roscpp</exec_depend>
   <exec_depend>rospy</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>moveit_core</exec_depend>
   <exec_depend>moveit_ros_planning_interface</exec_depend>
   <exec_depend>moveit_visual_tools</exec_depend>
+  <exec_depend>omnibot_manipulator_control</exec_depend>
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <!-- Other tools can request additional information be placed here -->
+    <cpp cflags="-I${prefix}/srv/cpp" />
 
   </export>
 </package>
diff --git a/src/omnibot_core/src/omnibot_core_lib/omnibot_core_lib.cpp b/src/omnibot_core/src/omnibot_core_lib/omnibot_core_lib.cpp
index 544c996..44c21b0 100644
--- a/src/omnibot_core/src/omnibot_core_lib/omnibot_core_lib.cpp
+++ b/src/omnibot_core/src/omnibot_core_lib/omnibot_core_lib.cpp
@@ -2,13 +2,56 @@
 
 OMNIBOT_CORE_LIB::OMNIBOT_CORE_LIB(ros::NodeHandle *nh, std::string config_path)
 {
+    _cfg = YAML::LoadFile(config_path);
+    _nh = nh;
 
-}
+    if (_cfg == NULL)
+    {
+        ROS_FATAL("CONFIG FILE LOAD FAILED!!!");
+    }
+    else
+    {
+        ROS_INFO("CONFIG FILE LOAD SUCCESS!!!");
+    }
+
+    if (_cfg["manipulator_on"].as<bool>())
+    {
+        ROS_INFO("Creating manipulator_cmd service...");
+        _manipulator_service_name = _cfg["manipulator_cmd_service_name"].as<std::string>();
+        _manipulator_service_client = nh->serviceClient<omnibot_manipulator_control::manipulator_cmd::Request,
+                                                        omnibot_manipulator_control::manipulator_cmd::Response>(_manipulator_service_name);
+        ROS_INFO("Creating manipulator_cmd service...DONE");
+        ROS_INFO("Waiting manipulator_cmd service server...");
+        _manipulator_service_client.waitForExistence();
+        ROS_INFO("Waiting manipulator_cmd service server...DONE");
 
+        ROS_INFO("Creating gripper_cmd service...");
+        _gripper_service_name = _cfg["gripper_cmd_service_name"].as<std::string>();
+        _gripper_service_client = nh->serviceClient<omnibot_manipulator_control::gripper_cmd::Request,
+                                                    omnibot_manipulator_control::gripper_cmd::Response>(_gripper_service_name);
+        ROS_INFO("Creating gripper_cmd service...DONE");
+        ROS_INFO("Waiting gripper_cmd service server...");
+        _gripper_service_client.waitForExistence();
+        ROS_INFO("Waiting gripper_cmd service server...DONE");
 
+        ROS_INFO("Send manipulator to home pose...");
+        omnibot_manipulator_control::manipulator_cmd::Request manip_requset;
+        omnibot_manipulator_control::manipulator_cmd::Response manip_response;
+        manip_requset.command_type = manip_requset.SAVED_POSE;
+        manip_requset.command = "home";
+        _manipulator_service_client.call(manip_requset, manip_response);
+        ROS_INFO("Send manipulator to home pose...%s", manip_response.result ? "DONE" : "FAILED");
 
+        ROS_INFO("Send gripper to home pose...");
+        omnibot_manipulator_control::gripper_cmd::Request gripper_requset;
+        omnibot_manipulator_control::gripper_cmd::Response gripper_response;
+        gripper_requset.command_type = gripper_requset.SAVED_POSE;
+        gripper_requset.command = "open";
+        _gripper_service_client.call(gripper_requset, gripper_response);
+        ROS_INFO("Send gripper to home pose...%s", gripper_response.result ? "DONE" : "FAILED");
+    }
+}
 
 OMNIBOT_CORE_LIB::~OMNIBOT_CORE_LIB()
 {
 }
-
diff --git a/src/omnibot_core/src/state_machine_lib/state_machine_lib.cpp b/src/omnibot_core/src/state_machine_lib/state_machine_lib.cpp
index 35184fc..05435fa 100644
--- a/src/omnibot_core/src/state_machine_lib/state_machine_lib.cpp
+++ b/src/omnibot_core/src/state_machine_lib/state_machine_lib.cpp
@@ -1,6 +1,6 @@
 #include "state_machine_lib/state_machine_lib.h"
 
-STATE_MACHINE_LIB::STATE_MACHINE_LIB(/* args */)
+STATE_MACHINE_LIB::STATE_MACHINE_LIB(ros::NodeHandle *nh)
 {
 }
 
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/CMakeLists.txt b/src/omnibot_manipulator/omnibot_manipulator_control/CMakeLists.txt
index 8783524..1b57eea 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_control/CMakeLists.txt
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/CMakeLists.txt
@@ -161,11 +161,13 @@ add_library(${PROJECT_NAME}
   src/${PROJECT_NAME}_server_lib/omnibot_manipulator_control_server_lib.cpp
   src/action_server_lib/action_server_lib.cpp
   )
-
+add_library(${PROJECT_NAME}_server_lib
+  src/${PROJECT_NAME}_server_lib/omnibot_manipulator_control_server_lib.cpp
+)
 ## Add cmake target dependencies of the library
 ## as an example, code may need to be generated before libraries
 ## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
@@ -208,24 +210,26 @@ target_link_libraries(${PROJECT_NAME}_server_node
 
 ## Mark executables for installation
 ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
+install(TARGETS ${PROJECT_NAME}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
 
+catkin_package(INCLUDE_DIRS include
+               LIBRARIES omnibot_manipulator_control_server_lib)
 ## Mark libraries for installation
 ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
-# )
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
 
 ## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+)
 
 ## Mark other files for installation (e.g. launch and bag files, etc.)
 # install(FILES
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp b/src/omnibot_manipulator/omnibot_manipulator_control/src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp
index 8c2da6f..90f0990 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_control/src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp
@@ -87,11 +87,9 @@ OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB(r
                                                                _actuators_trajectory_topic_name);
 
     _trajectory_followers_enable_service = _nh->advertiseService(
-        _trajectory_followers_enable_service_name, 
+        _trajectory_followers_enable_service_name,
         &OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::_trajectory_followers_enable_service_cb_f,
         this);
-    
-
 
     _rviz_goal_state_updater_pub = _nh->advertise<std_msgs::Empty>("/rviz/moveit/update_goal_state", 10);
 }
@@ -184,7 +182,6 @@ bool OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::ManipMoveByJointValues(std::vector<
 
     ROS_INFO("Planning for joints positions [%.2lf %.2lf %.2lf %.2lf %.2lf]",
              joint_values[0], joint_values[1], joint_values[2], joint_values[3], joint_values[4]);
-    updateGoalState();
     bool success = (_move_group_interface_arm->plan(_arm_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
     // _arm_plan.trajectory_.joint_trajectory.
     ROS_INFO("Planning %s", success ? "SUCCESS" : "FAILED");
@@ -313,6 +310,8 @@ bool OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::_grp_cmd_service_server_cb_f(omnibo
         break;
     }
     }
+    updateGoalState();
+
     return true;
 }
 
@@ -392,12 +391,13 @@ bool OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::_arm_cmd_service_server_cb_f(
         break;
     }
     }
+    updateGoalState();
+
     return true;
 }
 
-
 bool OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::_trajectory_followers_enable_service_cb_f(std_srvs::SetBool::Request &req,
-                                                   std_srvs::SetBool::Response &res)
+                                                                                       std_srvs::SetBool::Response &res)
 {
     _arm_trajectory_follower->setState((bool)req.data);
     _gripper_trajectory_follower->setState((bool)req.data);
@@ -406,7 +406,6 @@ bool OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::_trajectory_followers_enable_servic
     return true;
 }
 
-
 OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::~OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB()
 {
 }
\ No newline at end of file

From 5c04b55bd3a4ddf2a6864d7539a63894f763f977 Mon Sep 17 00:00:00 2001
From: IvanSnap360 <ivpetr2003@gmail.com>
Date: Sat, 10 Jun 2023 09:35:27 +0300
Subject: [PATCH 09/13] some changes on manipulator packages

---
 ...bot_manipulator_control_server_config.yaml |  2 +
 ...anipulator_dynamixel_contoller_config.yaml |  3 -
 .../launch/omnibot_manipulator_control.launch | 23 +++++---
 ...bot_manipulator_dynamixel_contoller.launch | 13 -----
 ...omnibot_manipulator_control_server_lib.cpp | 11 ++--
 .../CMakeLists.txt                            | 27 +++++----
 ...{dyn_config.yaml => dynamixel_config.yaml} |  0
 .../omnibot_manipulator_hw_controller_lib.h   |  0
 ...amixel.launch => dynamixel_control.launch} |  6 +-
 .../omnibot_manipulator_hw_controller.launch  | 10 +++-
 .../src/action_server.cpp                     | 57 -------------------
 .../omnibot_manipulator_hw_controller.cpp     |  0
 ...omnibot_manipulator_hw_controller_node.cpp | 27 +++++++--
 13 files changed, 67 insertions(+), 112 deletions(-)
 delete mode 100644 src/omnibot_manipulator/omnibot_manipulator_control/config/omnibot_manipulator_dynamixel_contoller_config.yaml
 delete mode 100644 src/omnibot_manipulator/omnibot_manipulator_control/launch/omnibot_manipulator_dynamixel_contoller.launch
 rename src/omnibot_manipulator/omnibot_manipulator_hw_controller/config/{dyn_config.yaml => dynamixel_config.yaml} (100%)
 delete mode 100644 src/omnibot_manipulator/omnibot_manipulator_hw_controller/include/omnibot_manipulator_hw_controller_lib/omnibot_manipulator_hw_controller_lib.h
 rename src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/{dynamixel.launch => dynamixel_control.launch} (91%)
 delete mode 100644 src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/action_server.cpp
 delete mode 100644 src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/omnibot_manipulator_hw_controller_lib/omnibot_manipulator_hw_controller.cpp

diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/config/omnibot_manipulator_control_server_config.yaml b/src/omnibot_manipulator/omnibot_manipulator_control/config/omnibot_manipulator_control_server_config.yaml
index 7bb3261..03a510e 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_control/config/omnibot_manipulator_control_server_config.yaml
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/config/omnibot_manipulator_control_server_config.yaml
@@ -24,7 +24,9 @@ hardware:
   default_state: false # on / off on startup
   trajectoryFollowersEnableServiceName: trajectory_followers_enable
   input:
+    manipulator_joint_state_input_topic: 
     manipulator_action_server_name: /maipulator_group_controller/follow_joint_trajectory
     gripper_action_server_name:     /gripper_group_controller/follow_joint_trajectory
   output:
+    manipulator_joint_state_output_topic:
     actuators_trajectory_topic: /dynamixel_contoller/joint_trajectory
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/config/omnibot_manipulator_dynamixel_contoller_config.yaml b/src/omnibot_manipulator/omnibot_manipulator_control/config/omnibot_manipulator_dynamixel_contoller_config.yaml
deleted file mode 100644
index 2a4e4eb..0000000
--- a/src/omnibot_manipulator/omnibot_manipulator_control/config/omnibot_manipulator_dynamixel_contoller_config.yaml
+++ /dev/null
@@ -1,3 +0,0 @@
-connection_params:
-  port: /dev/ttyUSB0
-  dxl_baud: 115200
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/launch/omnibot_manipulator_control.launch b/src/omnibot_manipulator/omnibot_manipulator_control/launch/omnibot_manipulator_control.launch
index 8b03d6d..fdcf13d 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_control/launch/omnibot_manipulator_control.launch
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/launch/omnibot_manipulator_control.launch
@@ -1,13 +1,14 @@
 <?xml version="1.0"?>
 <launch>
-    <arg name="namespace" default="omnibot_manipulator"/>
-    
-    <rosparam file="$(find omnibot_manipulator_control)/config/config.yaml" command="load" ns="$(arg namespace)" />
-    
-    <node name="controller_spawner_angle" pkg="controller_manager" type="spawner" respawn="false"
-        output="screen"
-        ns="$(arg namespace)"
-        args="
+  <arg name="namespace" default="omnibot_manipulator" />
+
+  <rosparam file="$(find omnibot_manipulator_control)/config/config.yaml" command="load"
+    ns="$(arg namespace)" />
+
+  <node name="controller_spawner_angle" pkg="controller_manager" type="spawner" respawn="false"
+    output="screen"
+    ns="$(arg namespace)"
+    args="
       angle_joint1_controller 
       angle_joint2_controller 
       angle_joint3_controller 
@@ -16,5 +17,9 @@
       angle_gripper_controller 
       angle_gripper_sub_controller" />
 
-
+  <include file="$(find omnibot_manipulator_control)/launch/dynamixel.launch">
+    <arg name="usb_port" default="/dev/ttyACM0" />
+    <arg name="dxl_baud_rate" default="1000000" />
+    <arg name="namespace" default="dynamixel_contoller" />
+  </include>
 </launch>
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/launch/omnibot_manipulator_dynamixel_contoller.launch b/src/omnibot_manipulator/omnibot_manipulator_control/launch/omnibot_manipulator_dynamixel_contoller.launch
deleted file mode 100644
index 725bc45..0000000
--- a/src/omnibot_manipulator/omnibot_manipulator_control/launch/omnibot_manipulator_dynamixel_contoller.launch
+++ /dev/null
@@ -1,13 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-    <arg name="hw_connection" default="false"/>
-    
-    <param name="omnibot_manipulator_control_node_config"
-        value="$(find omnibot_manipulator_control)/config/omnibot_manipulator_controller_config.yaml" />
-
-    <node pkg="omnibot_manipulator_control" type="omnibot_manipulator_control_node" name="omnibot_manipulator_control_node"
-        output="screen">
-        <param name="hw_connection" value="$(arg hw_connection)"/>
-    </node>
-
-</launch>
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp b/src/omnibot_manipulator/omnibot_manipulator_control/src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp
index 90f0990..200309e 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_control/src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp
@@ -72,11 +72,11 @@ OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB(r
     // ##################################################################### //
     // ######################## READ HARDWARE CONFIG ####################### //
     // ##################################################################### //
-    _trajectory_followers_state = _cfg["hardware"]["default_state"].as<bool>();
-    _trajectory_followers_enable_service_name = _cfg["hardware"]["trajectoryFollowersEnableServiceName"].as<std::string>();
-    _manipulator_action_server_name = _cfg["hardware"]["input"]["manipulator_action_server_name"].as<std::string>();
-    _gripper_action_server_name = _cfg["hardware"]["input"]["gripper_action_server_name"].as<std::string>();
-    _actuators_trajectory_topic_name = _cfg["hardware"]["output"]["actuators_trajectory_topic"].as<std::string>();
+    _trajectory_followers_state =                   _cfg["hardware"]["default_state"].as<bool>();
+    _trajectory_followers_enable_service_name =     _cfg["hardware"]["trajectoryFollowersEnableServiceName"].as<std::string>();
+    _manipulator_action_server_name =               _cfg["hardware"]["input"]["manipulator_action_server_name"].as<std::string>();
+    _gripper_action_server_name =                   _cfg["hardware"]["input"]["gripper_action_server_name"].as<std::string>();
+    _actuators_trajectory_topic_name =              _cfg["hardware"]["output"]["actuators_trajectory_topic"].as<std::string>();
 
     _arm_trajectory_follower = new RobotTrajectoryFollower(_nh,
                                                            _manipulator_action_server_name,
@@ -406,6 +406,7 @@ bool OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::_trajectory_followers_enable_servic
     return true;
 }
 
+
 OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::~OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB()
 {
 }
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/CMakeLists.txt b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/CMakeLists.txt
index 8ba3205..ce74273 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/CMakeLists.txt
+++ b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/CMakeLists.txt
@@ -11,8 +11,8 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
   std_msgs
-  actionlib_msgs
-  actionlib
+  # actionlib_msgs
+  # actionlib
   message_generation
 
 )
@@ -72,11 +72,11 @@ find_package(Boost REQUIRED COMPONENTS system)
 # )
 
 ## Generate added messages and services with any dependencies listed here
-generate_messages(
-  DEPENDENCIES
-  std_msgs
-  actionlib_msgs
-)
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+#   actionlib_msgs
+# )
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -108,9 +108,9 @@ generate_messages(
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
- INCLUDE_DIRS include
- LIBRARIES omnibot_manipulator_hw_controller
- CATKIN_DEPENDS roscpp rospy std_msgs actionlib_msgs actionlib
+#  INCLUDE_DIRS include
+#  LIBRARIES omnibot_manipulator_hw_controller
+#  CATKIN_DEPENDS roscpp rospy std_msgs actionlib_msgs actionlib
 #  DEPENDS system_lib 
 )
 
@@ -139,7 +139,6 @@ include_directories(
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
 add_executable(${PROJECT_NAME}_node src/omnibot_manipulator_hw_controller_node.cpp)
-add_executable(action_server  src/action_server.cpp)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -155,9 +154,9 @@ add_executable(action_server  src/action_server.cpp)
 target_link_libraries(${PROJECT_NAME}_node
   ${catkin_LIBRARIES}
 )
-target_link_libraries(action_server
-  ${catkin_LIBRARIES}
-)
+# target_link_libraries(action_server
+#   ${catkin_LIBRARIES}
+# )
 
 #############
 ## Install ##
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/config/dyn_config.yaml b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/config/dynamixel_config.yaml
similarity index 100%
rename from src/omnibot_manipulator/omnibot_manipulator_hw_controller/config/dyn_config.yaml
rename to src/omnibot_manipulator/omnibot_manipulator_hw_controller/config/dynamixel_config.yaml
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/include/omnibot_manipulator_hw_controller_lib/omnibot_manipulator_hw_controller_lib.h b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/include/omnibot_manipulator_hw_controller_lib/omnibot_manipulator_hw_controller_lib.h
deleted file mode 100644
index e69de29..0000000
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/dynamixel.launch b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/dynamixel_control.launch
similarity index 91%
rename from src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/dynamixel.launch
rename to src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/dynamixel_control.launch
index 6028d87..73daf1b 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/dynamixel.launch
+++ b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/dynamixel_control.launch
@@ -15,9 +15,9 @@
     <param name="use_joint_states_topic"  value="$(arg use_joint_state)"/>
     <param name="use_cmd_vel_topic"       value="$(arg use_cmd_vel)"/>
     <rosparam>
-      publish_period: 0.001
-      dxl_read_period: 0.001
-      dxl_write_period: 0.001
+      publish_period: 0.005
+      dxl_read_period: 0.005
+      dxl_write_period: 0.005
     </rosparam>
   </node>
 </launch>
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/omnibot_manipulator_hw_controller.launch b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/omnibot_manipulator_hw_controller.launch
index 1790061..41f34bf 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/omnibot_manipulator_hw_controller.launch
+++ b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/omnibot_manipulator_hw_controller.launch
@@ -5,8 +5,12 @@
         <arg name="dxl_baud_rate" default="1000000" />
         <arg name="namespace" default="dynamixel_contoller" />
     </include>
+
+    
     <node pkg="omnibot_manipulator_hw_controller" type="omnibot_manipulator_hw_controller_node"
-        name="omnibot_manipulator_hw_controller_node" output="screen" />
-    <node pkg="omnibot_manipulator_hw_controller" type="action_server"
-        name="omnibot_manipulator_hw_controller_action_server" output="screen" />
+        name="omnibot_manipulator_hw_controller_node" output="screen">
+        <param name="input_joint_states_topic_name" value="/dynamixel_contoller/joint_states" />
+        <param name="output_joint_states_topic_name"
+            value="/omnibot_robot/manipulator_joint_states_corrected" />
+    </node>
 </launch>
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/action_server.cpp b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/action_server.cpp
deleted file mode 100644
index f9d3ecf..0000000
--- a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/action_server.cpp
+++ /dev/null
@@ -1,57 +0,0 @@
-#include <ros/ros.h>
-#include <actionlib/server/simple_action_server.h>
-#include <control_msgs/FollowJointTrajectoryAction.h>
-#include <trajectory_msgs/JointTrajectory.h>
-
-class RobotTrajectoryFollower
-{
-protected:
-    ros::NodeHandle nh_;
-    // NodeHandle instance must be created before this line. Otherwise strange error may occur.
-    actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_;
-    ros::Publisher _trajectory_publisher;
-    std::string action_name_;
-
-public:
-    RobotTrajectoryFollower(std::string name,std::string traj_pub_name) : as_(nh_, name, false),
-                                                action_name_(name)
-    {
-        // Register callback functions:
-        as_.registerGoalCallback(boost::bind(&RobotTrajectoryFollower::goalCB, this));
-        as_.registerPreemptCallback(boost::bind(&RobotTrajectoryFollower::preemptCB, this));
-
-        as_.start();
-        _trajectory_publisher = nh_.advertise<trajectory_msgs::JointTrajectory>(traj_pub_name,10);
-    }
-
-    ~RobotTrajectoryFollower(void) // Destructor
-    {
-    }
-
-    void goalCB()
-    {
-        // accept the new goal
-        // goal_ = as_.acceptNewGoal()->samples;
-        std::cout << "goalCB!!!" << std::endl;
-        _trajectory_publisher.publish(as_.acceptNewGoal()->trajectory);
-    }
-
-    void preemptCB()
-    {
-        ROS_INFO("%s: Preempted", action_name_.c_str());
-        // set the action state to preempted
-        as_.setPreempted();
-    }
-};
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "action_server");
-
-    RobotTrajectoryFollower ManipulatorTrajectoryFollower("/maipulator_group_controller/follow_joint_trajectory","/dynamixel_contoller/joint_trajectory");
-    RobotTrajectoryFollower GripperTrajectoryFollower("/gripper_group_controller/follow_joint_trajectory","/dynamixel_contoller/joint_trajectory");
-
-    ros::spin();
-
-    return 0;
-}
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/omnibot_manipulator_hw_controller_lib/omnibot_manipulator_hw_controller.cpp b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/omnibot_manipulator_hw_controller_lib/omnibot_manipulator_hw_controller.cpp
deleted file mode 100644
index e69de29..0000000
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/omnibot_manipulator_hw_controller_node.cpp b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/omnibot_manipulator_hw_controller_node.cpp
index 0f819a0..8c6d064 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/omnibot_manipulator_hw_controller_node.cpp
+++ b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/omnibot_manipulator_hw_controller_node.cpp
@@ -3,11 +3,9 @@
 #include "sensor_msgs/JointState.h"
 #include <control_msgs/FollowJointTrajectoryAction.h>
 
-
 ros::Subscriber dynamixel_controller_joint_state_subscriber;
 ros::Publisher manipulator_joint_state_publisher;
 
-
 float map(float x, float in_min, float in_max, float out_min, float out_max)
 {
     return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
@@ -25,13 +23,32 @@ void dynamixel_controller_joint_state_subscriber_cb_f(const sensor_msgs::JointSt
 int main(int argc, char *argv[])
 {
     ros::init(argc, argv, "omnibotmanipulator_hw_controller_node");
-    ros::NodeHandle nh;
+    ros::NodeHandle nh("~");
+    std::string dynamixel_controller_joint_state_subscriber_name;
+    std::string manipulator_joint_state_publisher_name;
+    if (ros::param::has("~input_joint_states_topic_name"))
+    {
+        ros::param::get("~input_joint_states_topic_name", dynamixel_controller_joint_state_subscriber_name);
+    }
+    else
+    {
+        ROS_ERROR("Not set param 'input_joint_states_topic_name' ");
+    }
+
+    if (ros::param::has("~output_joint_states_topic_name"))
+    {
+        ros::param::get("~output_joint_states_topic_name", manipulator_joint_state_publisher_name);
+    }
+    else
+    {
+        ROS_ERROR("Not set param 'output_joint_states_topic_name' ");
+    }
 
     dynamixel_controller_joint_state_subscriber =
-        nh.subscribe<sensor_msgs::JointState>("/dynamixel_contoller/joint_states", 10,
+        nh.subscribe<sensor_msgs::JointState>(dynamixel_controller_joint_state_subscriber_name, 10,
                                               &dynamixel_controller_joint_state_subscriber_cb_f);
 
-    manipulator_joint_state_publisher = nh.advertise<sensor_msgs::JointState>("/omnibot_robot/manipulator_joint_states_correcter", 10);
+    manipulator_joint_state_publisher = nh.advertise<sensor_msgs::JointState>(manipulator_joint_state_publisher_name, 10);
     ros::spin();
     ros::shutdown();
     return 0;

From 02a5074381ed2cae008fd7b43cdd8450be6258b0 Mon Sep 17 00:00:00 2001
From: IvanSnap360 <ivpetr2003@gmail.com>
Date: Sat, 10 Jun 2023 09:37:48 +0300
Subject: [PATCH 10/13] change in startup launch files

---
 src/omnibot_bringup/launch/omnibot_real.launch                | 4 +++-
 .../launch/dynamixel_control.launch                           | 2 +-
 2 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/omnibot_bringup/launch/omnibot_real.launch b/src/omnibot_bringup/launch/omnibot_real.launch
index 3ee540d..9926c42 100644
--- a/src/omnibot_bringup/launch/omnibot_real.launch
+++ b/src/omnibot_bringup/launch/omnibot_real.launch
@@ -36,7 +36,9 @@
         <param name="baud" value="115200"/>
     </node>
 
-    
+    <include file="$(find omnibot_manipulator_hw_controller)/launch/omnibot_manipulator_hw_controller.launch"/>
+
+    <include file="$(find omnibot_core)/launch/omnibot_core.launch"/>
     <!--  -->
     <include file="$(find sensor_odom_manager)/launch/sensor_odom_manager.launch"/>
 
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/dynamixel_control.launch b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/dynamixel_control.launch
index 73daf1b..f5e704c 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/dynamixel_control.launch
+++ b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/dynamixel_control.launch
@@ -7,7 +7,7 @@
   <arg name="use_joint_state"         default="true"/>
   <arg name="use_cmd_vel"             default="false"/>
 
-  <param name="dynamixel_info"          value="$(find omnibot_manipulator_hw_controller)/config/dyn_config.yaml"/>
+  <param name="dynamixel_info"          value="$(find omnibot_manipulator_hw_controller)/config/dynamixel_config.yaml"/>
 
   <node name="$(arg namespace)" pkg="dynamixel_workbench_controllers" type="dynamixel_workbench_controllers"
         required="true" output="screen" args="$(arg usb_port) $(arg dxl_baud_rate)">

From b76d8e52dc038a9a48611ccf4cb7847d228b7ba3 Mon Sep 17 00:00:00 2001
From: IvanSnap360 <ivpetr2003@gmail.com>
Date: Sat, 10 Jun 2023 14:56:36 +0300
Subject: [PATCH 11/13] action server fix

---
 src/omnibot_bringup/rviz/rviz.rviz            |   15 +-
 .../CMakeLists.txt                            |  206 ++++
 .../launch/omnibot_manipulator_bringup.launch |   22 +
 .../omnibot_manipulator_bringup/package.xml   |   68 ++
 .../CMakeLists.txt                            |    6 +
 ...bot_manipulator_control_server_config.yaml |   12 -
 .../omnibot_manipulator_control_server_lib.h  |   13 +-
 .../launch/action_server.launch               |   10 +
 .../launch/omnibot_manipulator_control.launch |    6 -
 .../action_server_lib/action_server_lib.cpp   |    2 +-
 .../src/action_server_node.cpp                |   44 +
 ...omnibot_manipulator_control_server_lib.cpp |   28 -
 .../urdf/d435.urdf.xacro                      |  237 ++++
 .../config/dynamixel_config.yaml              |   11 +-
 .../omnibot_manipulator_hw_controller.launch  |    7 +-
 .../src/dynamixel_manager.cpp                 |   11 +
 .../config/sensors_3d.yaml                    |   27 +-
 .../launch/demo.launch                        |    2 +-
 .../launch/move_group.launch                  |    2 +-
 .../launch/moveit.rviz                        | 1074 ++++++++++++++++-
 ...bot_robot_moveit_sensor_manager.launch.xml |    1 -
 ...nager.launch.xml => sensor_manager.launch} |    0
 .../launch/rtabmap.launch                     |   12 +-
 23 files changed, 1716 insertions(+), 100 deletions(-)
 create mode 100644 src/omnibot_manipulator/omnibot_manipulator_bringup/CMakeLists.txt
 create mode 100644 src/omnibot_manipulator/omnibot_manipulator_bringup/launch/omnibot_manipulator_bringup.launch
 create mode 100644 src/omnibot_manipulator/omnibot_manipulator_bringup/package.xml
 create mode 100644 src/omnibot_manipulator/omnibot_manipulator_control/launch/action_server.launch
 create mode 100644 src/omnibot_manipulator/omnibot_manipulator_control/src/action_server_node.cpp
 create mode 100644 src/omnibot_manipulator/omnibot_manipulator_description/urdf/d435.urdf.xacro
 create mode 100644 src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/dynamixel_manager.cpp
 rename src/omnibot_manipulator/omnibot_manipulator_moveit/launch/{sensor_manager.launch.xml => sensor_manager.launch} (100%)

diff --git a/src/omnibot_bringup/rviz/rviz.rviz b/src/omnibot_bringup/rviz/rviz.rviz
index f438df7..cb14c06 100644
--- a/src/omnibot_bringup/rviz/rviz.rviz
+++ b/src/omnibot_bringup/rviz/rviz.rviz
@@ -6,7 +6,6 @@ Panels:
       Expanded:
         - /Global Options1
         - /Status1
-        - /3DMap1
         - /Robot1/LaserScan1
         - /move_base1/TEB_planner1
         - /move_base1/CostmapPlanner1/cost_cloud1
@@ -2686,7 +2685,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 4.92646598815918
+      Distance: 0.7304148077964783
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -2694,17 +2693,17 @@ Visualization Manager:
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: -0.28880730271339417
-        Y: 0.11647143214941025
-        Z: 0.5200644731521606
+        X: -0.04267846792936325
+        Y: 0.2888927161693573
+        Z: 0.13738508522510529
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5503977537155151
+      Pitch: -0.12460232526063919
       Target Frame: base_link
-      Yaw: 1.573593020439148
+      Yaw: 2.0085926055908203
     Saved: ~
 Window Geometry:
   Displays:
@@ -2718,7 +2717,7 @@ Window Geometry:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
diff --git a/src/omnibot_manipulator/omnibot_manipulator_bringup/CMakeLists.txt b/src/omnibot_manipulator/omnibot_manipulator_bringup/CMakeLists.txt
new file mode 100644
index 0000000..b8f8e74
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_bringup/CMakeLists.txt
@@ -0,0 +1,206 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(omnibot_manipulator_bringup)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES omnibot_manipulator_bringup
+#  CATKIN_DEPENDS roscpp rospy std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/omnibot_manipulator_bringup.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/omnibot_manipulator_bringup_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_omnibot_manipulator_bringup.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/src/omnibot_manipulator/omnibot_manipulator_bringup/launch/omnibot_manipulator_bringup.launch b/src/omnibot_manipulator/omnibot_manipulator_bringup/launch/omnibot_manipulator_bringup.launch
new file mode 100644
index 0000000..8aa6b34
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_bringup/launch/omnibot_manipulator_bringup.launch
@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<launch>
+    <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}] [${node}]: ${message}"/>
+
+    <arg name="sim" default="false" />
+    <include file="$(find omnibot_manipulator_moveit)/launch/demo.launch">
+        <arg name="sim" value="$(arg sim)" />
+        <arg name="use_rviz" value="true" />
+    </include>
+
+
+    <include
+        file="$(find omnibot_manipulator_hw_controller)/launch/omnibot_manipulator_hw_controller.launch" />
+    <include
+        file="$(find omnibot_manipulator_control)/launch/omnibot_manipulator_control_server.launch" />
+    <!-- <include file="$(find omnibot_manipulator_control)/launch/omnibot_manipulator_control.launch" /> -->
+
+    <group unless="$(arg sim)">
+        <include file="$(find omnibot_manipulator_control)/launch/action_server.launch" />
+    </group>
+
+</launch>
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_bringup/package.xml b/src/omnibot_manipulator/omnibot_manipulator_bringup/package.xml
new file mode 100644
index 0000000..767605a
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_bringup/package.xml
@@ -0,0 +1,68 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>omnibot_manipulator_bringup</name>
+  <version>0.0.0</version>
+  <description>The omnibot_manipulator_bringup package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="ivan@todo.todo">ivan</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/omnibot_manipulator_bringup</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/CMakeLists.txt b/src/omnibot_manipulator/omnibot_manipulator_control/CMakeLists.txt
index 1b57eea..5bdfdb6 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_control/CMakeLists.txt
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/CMakeLists.txt
@@ -176,6 +176,8 @@ add_executable(${PROJECT_NAME}_server_node
   src/omnibot_manipulator_control_server_node.cpp 
   src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp
   src/action_server_lib/action_server_lib.cpp)
+
+add_executable(action_server_node src/action_server_node.cpp src/action_server_lib/action_server_lib.cpp)
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
 ## target back to the shorter version for ease of user use
@@ -192,6 +194,10 @@ target_link_libraries(${PROJECT_NAME}_server_node
   ${catkin_LIBRARIES}
 )
 
+target_link_libraries(action_server_node
+  yaml-cpp
+  ${catkin_LIBRARIES}
+)
 
 
 #############
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/config/omnibot_manipulator_control_server_config.yaml b/src/omnibot_manipulator/omnibot_manipulator_control/config/omnibot_manipulator_control_server_config.yaml
index 03a510e..344d850 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_control/config/omnibot_manipulator_control_server_config.yaml
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/config/omnibot_manipulator_control_server_config.yaml
@@ -18,15 +18,3 @@ services:
   gripper_cmd_service:
     name: gripper_cmd
     saved_poses: [{ name: "open", val: 0.025 }, { name: "close", val: 0.01 }]
-
-
-hardware:
-  default_state: false # on / off on startup
-  trajectoryFollowersEnableServiceName: trajectory_followers_enable
-  input:
-    manipulator_joint_state_input_topic: 
-    manipulator_action_server_name: /maipulator_group_controller/follow_joint_trajectory
-    gripper_action_server_name:     /gripper_group_controller/follow_joint_trajectory
-  output:
-    manipulator_joint_state_output_topic:
-    actuators_trajectory_topic: /dynamixel_contoller/joint_trajectory
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/include/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.h b/src/omnibot_manipulator/omnibot_manipulator_control/include/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.h
index 32bec28..ceab3b1 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_control/include/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.h
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/include/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.h
@@ -42,18 +42,7 @@ class OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB
 
     ros::ServiceServer _arm_cmd_service_server;
     ros::ServiceServer _gripper_cmd_service_server;
-
-    RobotTrajectoryFollower *_arm_trajectory_follower;
-    RobotTrajectoryFollower *_gripper_trajectory_follower;
-    bool _trajectory_followers_state = false;
-    std::string _trajectory_followers_enable_service_name;
-    std::string _manipulator_action_server_name;
-    std::string _gripper_action_server_name;
-    std::string _actuators_trajectory_topic_name;
-    ros::ServiceServer _trajectory_followers_enable_service;
-    bool _trajectory_followers_enable_service_cb_f(std_srvs::SetBool::Request &req,
-                                                   std_srvs::SetBool::Response &res);
-
+    
     bool ManipMoveByJointValues(std::vector<double> joint_values);
     bool ManipMoveByPosition(geometry_msgs::Pose new_pose);
     bool ManipMoveBySavedPosition(std::string pose_name);
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/launch/action_server.launch b/src/omnibot_manipulator/omnibot_manipulator_control/launch/action_server.launch
new file mode 100644
index 0000000..17b6775
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/launch/action_server.launch
@@ -0,0 +1,10 @@
+<?xml version="1.0"?>
+<launch>
+
+    <node pkg="omnibot_manipulator_control" type="action_server_node" name="action_server_node">
+        <param name="manipulator_action_name"   value="/maipulator_group_controller/follow_joint_trajectory"/>
+        <param name="gripper_action_name"       value="/gripper_group_controller/follow_joint_trajectory"/>
+        <param name="output_topic_name"         value="/dynamixel_contoller/joint_trajectory"/>
+    </node>
+
+</launch>
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/launch/omnibot_manipulator_control.launch b/src/omnibot_manipulator/omnibot_manipulator_control/launch/omnibot_manipulator_control.launch
index fdcf13d..481c180 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_control/launch/omnibot_manipulator_control.launch
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/launch/omnibot_manipulator_control.launch
@@ -16,10 +16,4 @@
       angle_joint5_controller 
       angle_gripper_controller 
       angle_gripper_sub_controller" />
-
-  <include file="$(find omnibot_manipulator_control)/launch/dynamixel.launch">
-    <arg name="usb_port" default="/dev/ttyACM0" />
-    <arg name="dxl_baud_rate" default="1000000" />
-    <arg name="namespace" default="dynamixel_contoller" />
-  </include>
 </launch>
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/src/action_server_lib/action_server_lib.cpp b/src/omnibot_manipulator/omnibot_manipulator_control/src/action_server_lib/action_server_lib.cpp
index 3e54b21..a77035b 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_control/src/action_server_lib/action_server_lib.cpp
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/src/action_server_lib/action_server_lib.cpp
@@ -3,7 +3,7 @@
 RobotTrajectoryFollower::RobotTrajectoryFollower(ros::NodeHandle *_nh, std::string action_server_name, std::string traj_pub_name)
 {
 
-    _as = new actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction>(action_server_name, true);
+    _as = new actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction>(action_server_name, false);
     // Register callback functions:
     _as->registerGoalCallback(boost::bind(&RobotTrajectoryFollower::goalCB, this));
     _as->registerPreemptCallback(boost::bind(&RobotTrajectoryFollower::preemptCB, this));
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/src/action_server_node.cpp b/src/omnibot_manipulator/omnibot_manipulator_control/src/action_server_node.cpp
new file mode 100644
index 0000000..82dbbd0
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/src/action_server_node.cpp
@@ -0,0 +1,44 @@
+#include <ros/ros.h>
+
+#include "action_server_lib/action_server_lib.h"
+
+int main(int argc, char *argv[])
+{
+    ros::init(argc, argv, "action_server_node");
+    ros::NodeHandle nh("~");
+    std::string manipulator_action_name;
+    std::string gripper_action_name;
+    std::string output_topic_name;
+    if (ros::param::has("~manipulator_action_name"))
+    {
+        ros::param::get("~manipulator_action_name", manipulator_action_name);
+    }
+    else
+    {
+        ROS_ERROR("No set param 'manipulator_action_name'!!!");
+    }
+    if (ros::param::has("~gripper_action_name"))
+    {
+        ros::param::get("~gripper_action_name", gripper_action_name);
+    }
+    else
+    {
+        ROS_ERROR("No set param 'gripper_action_name'!!!");
+    }
+    if (ros::param::has("~output_topic_name"))
+    {
+        ros::param::get("~output_topic_name", output_topic_name);
+    }
+    else
+    {
+        ROS_ERROR("No set param 'output_topic_name'!!!");
+    }
+
+    auto manipulator_server = RobotTrajectoryFollower(&nh, manipulator_action_name, output_topic_name);
+    auto gripper_server = RobotTrajectoryFollower(&nh, gripper_action_name, output_topic_name);
+    manipulator_server.setState(true);
+    gripper_server.setState(true);
+    ros::spin();
+    ros::shutdown();
+    return 0;
+}
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp b/src/omnibot_manipulator/omnibot_manipulator_control/src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp
index 200309e..c4ae71d 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_control/src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp
@@ -72,25 +72,6 @@ OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB(r
     // ##################################################################### //
     // ######################## READ HARDWARE CONFIG ####################### //
     // ##################################################################### //
-    _trajectory_followers_state =                   _cfg["hardware"]["default_state"].as<bool>();
-    _trajectory_followers_enable_service_name =     _cfg["hardware"]["trajectoryFollowersEnableServiceName"].as<std::string>();
-    _manipulator_action_server_name =               _cfg["hardware"]["input"]["manipulator_action_server_name"].as<std::string>();
-    _gripper_action_server_name =                   _cfg["hardware"]["input"]["gripper_action_server_name"].as<std::string>();
-    _actuators_trajectory_topic_name =              _cfg["hardware"]["output"]["actuators_trajectory_topic"].as<std::string>();
-
-    _arm_trajectory_follower = new RobotTrajectoryFollower(_nh,
-                                                           _manipulator_action_server_name,
-                                                           _actuators_trajectory_topic_name);
-
-    _gripper_trajectory_follower = new RobotTrajectoryFollower(_nh,
-                                                               _gripper_action_server_name,
-                                                               _actuators_trajectory_topic_name);
-
-    _trajectory_followers_enable_service = _nh->advertiseService(
-        _trajectory_followers_enable_service_name,
-        &OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::_trajectory_followers_enable_service_cb_f,
-        this);
-
     _rviz_goal_state_updater_pub = _nh->advertise<std_msgs::Empty>("/rviz/moveit/update_goal_state", 10);
 }
 
@@ -396,15 +377,6 @@ bool OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::_arm_cmd_service_server_cb_f(
     return true;
 }
 
-bool OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::_trajectory_followers_enable_service_cb_f(std_srvs::SetBool::Request &req,
-                                                                                       std_srvs::SetBool::Response &res)
-{
-    _arm_trajectory_follower->setState((bool)req.data);
-    _gripper_trajectory_follower->setState((bool)req.data);
-    res.message = "Trajectory followers is " + req.data ? "ON" : "OFF";
-    res.success = true;
-    return true;
-}
 
 
 OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::~OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB()
diff --git a/src/omnibot_manipulator/omnibot_manipulator_description/urdf/d435.urdf.xacro b/src/omnibot_manipulator/omnibot_manipulator_description/urdf/d435.urdf.xacro
new file mode 100644
index 0000000..f476a1a
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_description/urdf/d435.urdf.xacro
@@ -0,0 +1,237 @@
+<?xml version="1.0"?>
+<!--
+The MIT License
+
+Copyright (c) 2019 Giovanni di Dio Bruno https://gbr1.github.io
+
+Permission is
+hereby granted, free of charge, to any person obtaining a copy
+of this software and associated
+documentation files (the "Software"), to deal
+in the Software without restriction, including without
+limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to
+the following conditions:
+
+The above copyright notice and this permission notice shall be included
+in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT
+WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS
+OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF
+CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+OTHER DEALINGS IN
+THE SOFTWARE.
+-->
+<robot name="d400" xmlns:xacro="http://ros.org/wiki/xacro">
+    <xacro:macro name="d400" params="mesh_path parent_link *origin">
+        <xacro:property name="M_PI" value="3.1415926535897931" />
+        <xacro:property name="M_PI_2" value="1.570796327" />
+
+        <xacro:property name="d400_cam_depth_to_left_ir_offset" value="0.0" />
+        <xacro:property name="d400_cam_depth_to_right_ir_offset" value="-0.050" />
+        <xacro:property name="d400_cam_depth_to_color_offset" value="0.015" />
+
+        <xacro:property name="d400_cam_width" value="0.090" />
+        <xacro:property name="d400_cam_height" value="0.025" />
+        <xacro:property name="d400_cam_depth" value="0.02505" />
+        <xacro:property name="d400_cam_mount_from_center_offset" value="0.0149" />
+
+        <xacro:property name="d400_cam_depth_px" value="${d400_cam_mount_from_center_offset}" />
+        <xacro:property name="d400_cam_depth_py" value="0.0175" />
+        <xacro:property name="d400_cam_depth_pz" value="${d400_cam_height/2}" />
+
+
+        <joint name="d400_joint" type="fixed">
+            <parent link="${parent_link}" />
+            <child link="d400_bottom_screw_frame" />
+            <xacro:insert_block name="origin" />
+        </joint>
+        <link name="d400_bottom_screw_frame" />
+        <gazebo reference="d400_bottom_screw_frame">
+            <turnGravityOff>false</turnGravityOff>
+        </gazebo>
+
+        <joint name="d400_link_joint" type="fixed">
+            <origin xyz="0 ${d400_cam_depth_py} ${d400_cam_depth_pz}" rpy="0 0 0" />
+            <parent link="d400_bottom_screw_frame" />
+            <child link="d400_link" />
+        </joint>
+
+        <link name="d400_link">
+            <visual>
+                <origin xyz="${d400_cam_mount_from_center_offset} ${-d400_cam_depth_py} 0"
+                    rpy="${M_PI_2} 0 ${M_PI_2}" />
+                <geometry>
+                    <mesh filename="${mesh_path}" scale="0.001 0.001 0.001" />
+                </geometry>
+                <material name="">
+                    <color rgba="0.3 0.3 0.3 1.0" />
+                    <texture filename="" />
+                </material>
+            </visual>
+            <collision>
+                <origin xyz="0 ${-d400_cam_depth_py} 0" rpy="0 0 0" />
+                <geometry>
+                    <box size="${d400_cam_depth} ${d400_cam_width} ${d400_cam_height}" />
+                </geometry>
+            </collision>
+            <inertial>
+                <mass value="0.180" />
+                <origin xyz="0 0 0" />
+                <inertia ixx="0.003881243" ixy="0.0" ixz="0.0" iyy="0.000498940" iyz="0.0"
+                    izz="0.003879257" />
+            </inertial>
+        </link>
+        <gazebo reference="d400_link">
+            <turnGravityOff>false</turnGravityOff>
+            <material>Gazebo/Grey</material>
+        </gazebo>
+
+        <!-- d400 depth joints and links -->
+        <joint name="d400_depth_joint" type="fixed">
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <parent link="d400_link" />
+            <child link="d400_depth_frame" />
+        </joint>
+        <link name="d400_depth_frame" />
+        <gazebo reference="d400_depth_frame">
+            <turnGravityOff>false</turnGravityOff>
+        </gazebo>
+
+        <joint name="d400_depth_optical_joint" type="fixed">
+            <origin xyz="0 0 0" rpy="${-M_PI_2} 0 ${-M_PI_2}" />
+            <parent link="d400_depth_frame" />
+            <child link="d400_depth_optical_frame" />
+        </joint>
+        <link name="d400_depth_optical_frame" />
+        <gazebo reference="d400_depth_optical_frame">
+            <turnGravityOff>false</turnGravityOff>
+        </gazebo>
+
+        <!-- d400 left IR joints and links -->
+        <joint name="d400_left_ir_joint" type="fixed">
+            <origin xyz="0 ${d400_cam_depth_to_left_ir_offset} 0" rpy="0 0 0" />
+            <parent link="d400_depth_frame" />
+            <child link="d400_left_ir_frame" />
+        </joint>
+        <link name="d400_left_ir_frame" />
+        <gazebo reference="d400_left_ir_frame">
+            <turnGravityOff>false</turnGravityOff>
+        </gazebo>
+
+        <joint name="d400_left_ir_optical_joint" type="fixed">
+            <origin xyz="0 0 0" rpy="${-M_PI_2} 0 ${-M_PI_2}" />
+            <parent link="d400_left_ir_frame" />
+            <child link="d400_left_ir_optical_frame" />
+        </joint>
+        <link name="d400_left_ir_optical_frame" />
+        <gazebo reference="d400_left_ir_optical_frame">
+            <turnGravityOff>false</turnGravityOff>
+        </gazebo>
+
+        <!-- d400 right IR joints and links -->
+        <joint name="d400_right_ir_joint" type="fixed">
+            <origin xyz="0 ${d400_cam_depth_to_right_ir_offset} 0" rpy="0 0 0" />
+            <parent link="d400_depth_frame" />
+            <child link="d400_right_ir_frame" />
+        </joint>
+        <link name="d400_right_ir_frame" />
+        <gazebo reference="d400_right_ir_frame">
+            <turnGravityOff>false</turnGravityOff>
+        </gazebo>
+
+        <joint name="d400_right_ir_optical_joint" type="fixed">
+            <origin xyz="0 0 0" rpy="${-M_PI_2} 0 ${-M_PI_2}" />
+            <parent link="d400_right_ir_frame" />
+            <child link="d400_right_ir_optical_frame" />
+        </joint>
+        <link name="d400_right_ir_optical_frame" />
+        <gazebo reference="d400_right_ir_optical_frame">
+            <turnGravityOff>false</turnGravityOff>
+        </gazebo>
+
+        <!-- d400 color joints and links -->
+        <joint name="d400_color_joint" type="fixed">
+            <origin xyz="0 ${d400_cam_depth_to_color_offset} 0" rpy="0 0 0" />
+            <parent link="d400_depth_frame" />
+            <child link="d400_color_frame" />
+        </joint>
+        <link name="d400_color_frame" />
+        <gazebo reference="d400_color_frame">
+            <turnGravityOff>false</turnGravityOff>
+        </gazebo>
+
+        <joint name="d400_color_optical_joint" type="fixed">
+            <origin xyz="0 0 0" rpy="${-M_PI_2} 0 ${-M_PI_2}" />
+            <parent link="d400_color_frame" />
+            <child link="d400_color_optical_frame" />
+        </joint>
+        <link name="d400_color_optical_frame" />
+        <gazebo reference="d400_color_optical_frame">
+            <turnGravityOff>false</turnGravityOff>
+        </gazebo>
+
+        <link name="d400_depth_frame_gazebo" />
+        <joint name="gazebo_joint" type="fixed">
+            <origin xyz="0 0 0" rpy="${-M_PI_2} 0 ${-M_PI_2}" />
+            <parent link="d400_link" />
+            <child link="d400_depth_frame_gazebo" />
+        </joint>
+        <gazebo reference="d400_depth_frame_gazebo">
+            <turnGravityOff>false</turnGravityOff>
+        </gazebo>
+
+    </xacro:macro>
+
+    <gazebo reference="d400_link">
+        <sensor name="d400" type="depth">
+            <update_rate>30</update_rate>
+            <camera>
+                <horizontal_fov>1.59174</horizontal_fov>
+                <image>
+                    <width>640</width>
+                    <height>480</height>
+                    <format>R8G8B8</format>
+                </image>
+                <clip>
+                    <near>0.02</near>
+                    <far>6</far>
+                </clip>
+            </camera>
+            <plugin name="camera_plugin" filename="libgazebo_ros_openni_kinect.so">
+                <visualize>true</visualize>
+                <baseline>0.05</baseline>
+                <alwaysOn>true</alwaysOn>
+                <updateRate>30.0</updateRate>
+                <cameraName>d400</cameraName>
+                <imageTopicName>/d400/color/image_raw</imageTopicName>
+                <cameraInfoTopicName>/d400/color/camera_info</cameraInfoTopicName>
+                <depthImageTopicName>/d400/aligned_depth_to_color/image_raw</depthImageTopicName>
+                <depthImageInfoTopicName>/d400/depth/d400_info</depthImageInfoTopicName>
+                <pointCloudTopicName>/d400/depth/color/points</pointCloudTopicName>
+                <frameName>d400_depth_frame_gazebo</frameName>
+                <pointCloudCutoff>0.02</pointCloudCutoff>
+                <distortionK1>0</distortionK1>
+                <distortionK2>0</distortionK2>
+                <distortionK3>0</distortionK3>
+                <distortionT1>0</distortionT1>
+                <distortionT2>0</distortionT2>
+                <CxPrime>0</CxPrime>
+                <Cx>0</Cx>
+                <Cy>0</Cy>
+                <focalLength>0</focalLength>
+                <hackBaseline>0</hackBaseline>
+            </plugin>
+        </sensor>
+    </gazebo>
+</robot>
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/config/dynamixel_config.yaml b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/config/dynamixel_config.yaml
index 060bd12..37b096d 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/config/dynamixel_config.yaml
+++ b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/config/dynamixel_config.yaml
@@ -3,29 +3,38 @@ ang_joint1:
   Return_Delay_Time: 0
   CW_Angle_Limit: 0
   CCW_Angle_Limit: 4095
+  Moving_Speed: 40
 ang_joint2:
   ID: 2
   Return_Delay_Time: 0
   CW_Angle_Limit: 1023
   CCW_Angle_Limit: 3072
+  Moving_Speed: 40
+
 ang_joint3:
   ID: 3
   Return_Delay_Time: 0
   CW_Angle_Limit: 1023
   CCW_Angle_Limit: 3072
+  Moving_Speed: 40
+
 ang_joint4:
   ID: 4
   Return_Delay_Time: 0
   CW_Angle_Limit: 1023
   CCW_Angle_Limit: 3072
+  Moving_Speed: 40
+
 ang_joint5:
   ID: 5
   Return_Delay_Time: 0
   CW_Angle_Limit: 0
   CCW_Angle_Limit: 1023
+  Moving_Speed: 40
 
 ang_gripper:
   ID: 6
   Return_Delay_Time: 0
   CW_Angle_Limit: 0
-  CCW_Angle_Limit: 1023
\ No newline at end of file
+  CCW_Angle_Limit: 1023
+  Moving_Speed: 40
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/omnibot_manipulator_hw_controller.launch b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/omnibot_manipulator_hw_controller.launch
index 41f34bf..6c65021 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/omnibot_manipulator_hw_controller.launch
+++ b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/omnibot_manipulator_hw_controller.launch
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <launch>
-    <include file="$(find omnibot_manipulator_hw_controller)/launch/dynamixel.launch">
+    <include file="$(find omnibot_manipulator_hw_controller)/launch/dynamixel_control.launch">
         <arg name="usb_port" default="/dev/ttyACM0" />
         <arg name="dxl_baud_rate" default="1000000" />
         <arg name="namespace" default="dynamixel_contoller" />
@@ -9,8 +9,7 @@
     
     <node pkg="omnibot_manipulator_hw_controller" type="omnibot_manipulator_hw_controller_node"
         name="omnibot_manipulator_hw_controller_node" output="screen">
-        <param name="input_joint_states_topic_name" value="/dynamixel_contoller/joint_states" />
-        <param name="output_joint_states_topic_name"
-            value="/omnibot_robot/manipulator_joint_states_corrected" />
+        <param name="input_joint_states_topic_name"  value="/dynamixel_contoller/joint_states" />
+        <param name="output_joint_states_topic_name" value="/omnibot_robot/manipulator_joint_states_corrected" />
     </node>
 </launch>
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/dynamixel_manager.cpp b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/dynamixel_manager.cpp
new file mode 100644
index 0000000..aefef7f
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/dynamixel_manager.cpp
@@ -0,0 +1,11 @@
+#include <ros/ros.h>
+#include "dynamixel_workbench_msgs/DynamixelCommand.h"
+
+int main(int argc, char * argv[])
+{
+    ros::init(argc, argv, "/* node_name */");
+    auto /* node_name */ = /* namespace_name::ClassName */();
+    ros::spin();
+    ros::shutdown();
+    return 0;
+}
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/config/sensors_3d.yaml b/src/omnibot_manipulator/omnibot_manipulator_moveit/config/sensors_3d.yaml
index 51010a3..7bbd230 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/config/sensors_3d.yaml
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/config/sensors_3d.yaml
@@ -1,2 +1,27 @@
 sensors:
-  []
\ No newline at end of file
+  []
+
+# sensors: 
+#   - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
+#     # point_cloud_topic: /rtabmap/cloud_map
+#     # point_cloud_topic: /rtabmap/cloud_obstacles
+#     # point_cloud_topic: /rtabmap/local_grid_obstacle
+#     point_cloud_topic: /d400_arm/depth/color/points
+#     max_range: 2.0
+#     max_update_rate: 100.0
+#     point_subsample: 1
+#     padding_offset: 0.08
+#     padding_scale: 1.0
+#     filtered_cloud_topic: filtered_cloud
+
+  # - sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
+  #   image_topic: /d400_arm/aligned_depth_to_color/image_raw
+  #   queue_size: 5
+  #   near_clipping_plane_distance: 1.0
+  #   far_clipping_plane_distance: 5.0
+  #   shadow_threshold: 1.0
+  #   padding_scale: 0.25
+  #   padding_offset: 0.1
+  #   max_update_rate: 10.0
+  #   filtered_cloud_topic: filtered_cloud
+  #   ns: d400_arm
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/demo.launch b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/demo.launch
index 0fc39d8..db68289 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/demo.launch
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/demo.launch
@@ -39,7 +39,7 @@
     joint_state_publisher
          MoveIt's fake controller's joint states are considered via the 'source_list' parameter -->
       <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
-        <rosparam param="source_list">[/omnibot_robot/manipulator_joint_states_correcter]</rosparam>
+        <rosparam param="source_list">[/omnibot_robot/manipulator_joint_states_corrected]</rosparam>
       </node>
 
       <node name="omnibot_robot_state_publisher" pkg="robot_state_publisher"
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/move_group.launch b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/move_group.launch
index 50bf34e..1985c92 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/move_group.launch
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/move_group.launch
@@ -77,7 +77,7 @@
   </include>
 
   <!-- Sensors Functionality -->
-  <include ns="move_group" file="$(dirname)/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
+  <include ns="move_group" file="$(dirname)/sensor_manager.launch" if="$(arg allow_trajectory_execution)">
     <arg name="moveit_sensor_manager" value="omnibot_robot" />
   </include>
 
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/moveit.rviz b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/moveit.rviz
index 7410dea..a7e0178 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/moveit.rviz
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/moveit.rviz
@@ -1,12 +1,12 @@
 Panels:
   - Class: rviz/Displays
-    Help Height: 84
+    Help Height: 138
     Name: Displays
     Property Tree Widget:
       Expanded:
         - /MotionPlanning1
       Splitter Ratio: 0.5
-    Tree Height: 226
+    Tree Height: 138
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
@@ -14,6 +14,10 @@ Panels:
       - /Current View1
     Name: Views
     Splitter Ratio: 0.5
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
 Visualization Manager:
   Class: ""
   Displays:
@@ -23,7 +27,7 @@ Visualization Manager:
       Color: 160; 160; 164
       Enabled: true
       Line Style:
-        Line Width: 0.03
+        Line Width: 0.029999999329447746
         Value: Lines
       Name: Grid
       Normal Cell Count: 0
@@ -35,30 +39,557 @@ Visualization Manager:
       Plane Cell Count: 10
       Reference Frame: <Fixed Frame>
       Value: true
-    - Class: moveit_rviz_plugin/MotionPlanning
+    - Acceleration_Scaling_Factor: 0.1
+      Class: moveit_rviz_plugin/MotionPlanning
       Enabled: true
+      Move Group Namespace: ""
+      MoveIt_Allow_Approximate_IK: false
+      MoveIt_Allow_External_Program: false
+      MoveIt_Allow_Replanning: false
+      MoveIt_Allow_Sensor_Positioning: false
+      MoveIt_Planning_Attempts: 10
+      MoveIt_Planning_Time: 5
+      MoveIt_Use_Cartesian_Path: false
+      MoveIt_Use_Constraint_Aware_IK: false
+      MoveIt_Workspace:
+        Center:
+          X: 0
+          Y: 0
+          Z: 0
+        Size:
+          X: 2
+          Y: 2
+          Z: 2
       Name: MotionPlanning
       Planned Path:
-        Links: ~
+        Color Enabled: false
+        Interrupt Display: false
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          ang_link0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          base_footprint:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          d400_arm_bottom_screw_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_color_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_color_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_depth_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_depth_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_left_ir_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_left_ir_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          d400_arm_right_ir_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_right_ir_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_bottom_screw_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_color_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_color_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_depth_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_depth_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_left_ir_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_left_ir_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          d400_right_ir_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_right_ir_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          left_back_mecanum_adapter_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_mecanum_wheel_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_mecanum_adapter_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_mecanum_wheel_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_mecanum_adapter_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_mecanum_wheel_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_mecanum_adapter_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_mecanum_wheel_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          t265_accel_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_accel_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_fisheye1_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_fisheye1_rgb_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_fisheye2_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_fisheye2_rgb_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_gyro_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_gyro_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_pose_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          tip_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
         Loop Animation: true
         Robot Alpha: 0.5
+        Robot Color: 150; 50; 150
         Show Robot Collision: false
         Show Robot Visual: true
         Show Trail: false
         State Display Time: 0.05 s
+        Trail Step Size: 1
         Trajectory Topic: move_group/display_planned_path
+        Use Sim Time: false
       Planning Metrics:
         Payload: 1
         Show Joint Torques: false
         Show Manipulability: false
         Show Manipulability Index: false
         Show Weight Limit: false
+        TextHeight: 0.07999999821186066
       Planning Request:
         Colliding Link Color: 255; 0; 0
         Goal State Alpha: 1
         Goal State Color: 250; 128; 0
         Interactive Marker Size: 0
         Joint Violation Color: 255; 0; 255
+        Planning Group: maipulator_group
         Query Goal State: true
         Query Start State: false
         Show Workspace: false
@@ -68,19 +599,526 @@ Visualization Manager:
       Robot Description: robot_description
       Scene Geometry:
         Scene Alpha: 1
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.009999999776482582
         Show Scene Geometry: true
         Voxel Coloring: Z-Axis
         Voxel Rendering: Occupied Voxels
       Scene Robot:
         Attached Body Color: 150; 50; 150
-        Links: ~
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          ang_link0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ang_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          base_footprint:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          d400_arm_bottom_screw_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_color_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_color_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_depth_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_depth_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_left_ir_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_left_ir_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          d400_arm_right_ir_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_arm_right_ir_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_bottom_screw_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_color_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_color_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_depth_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_depth_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_left_ir_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_left_ir_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          d400_right_ir_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          d400_right_ir_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          left_back_mecanum_adapter_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_mecanum_wheel_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_1_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_roller_2_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_mecanum_adapter_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_mecanum_wheel_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_1_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_roller_2_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_mecanum_adapter_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_mecanum_wheel_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_1_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_roller_2_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_mecanum_adapter_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_mecanum_wheel_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_1_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_5_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_6_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_roller_2_7_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          t265_accel_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_accel_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_fisheye1_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_fisheye1_rgb_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_fisheye2_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_fisheye2_rgb_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_gyro_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_gyro_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          t265_pose_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          tip_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
         Robot Alpha: 0.5
-        Show Scene Robot: true
+        Show Robot Collision: false
+        Show Robot Visual: true
       Value: true
+      Velocity_Scaling_Factor: 0.1
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
+    Default Light: true
     Fixed Frame: base_footprint
+    Frame Rate: 30
   Name: root
   Tools:
     - Class: rviz/Interact
@@ -91,22 +1129,22 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 2.0
+      Distance: 2
       Enable Stereo Rendering:
-        Stereo Eye Separation: 0.06
+        Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Field of View: 0.75
       Focal Point:
-        X: -0.1
+        X: -0.10000000149011612
         Y: 0.25
-        Z: 0.30
+        Z: 0.30000001192092896
       Focal Shape Fixed Size: true
-      Focal Shape Size: 0.05
+      Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
-      Near Clip Distance: 0.01
+      Near Clip Distance: 0.009999999776482582
       Pitch: 0.5
       Target Frame: base_footprint
       Yaw: -0.6232355833053589
@@ -114,7 +1152,7 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 848
+  Height: 1267
   Help:
     collapsed: false
   Hide Left Dock: false
@@ -123,9 +1161,9 @@ Window Geometry:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000001f0000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d00000173000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b60000017d0000017d00ffffff00000315000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000100000000000003a300000457fc0200000007fb000000100044006900730070006c006100790073010000006e000001820000018200fffffffb0000000800480065006c00700000000342000000bb000000d400fffffffb0000000a00560069006500770073000000010c000000a40000013200fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000002600000026fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001fc000002c9000002b800ffffff000003e60000045700000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Views:
     collapsed: false
-  Width: 1291
-  X: 454
-  Y: 25
+  Width: 1941
+  X: 452
+  Y: 54
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/omnibot_robot_moveit_sensor_manager.launch.xml b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/omnibot_robot_moveit_sensor_manager.launch.xml
index 5d02698..6446752 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/omnibot_robot_moveit_sensor_manager.launch.xml
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/omnibot_robot_moveit_sensor_manager.launch.xml
@@ -1,3 +1,2 @@
 <launch>
-
 </launch>
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/sensor_manager.launch.xml b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/sensor_manager.launch
similarity index 100%
rename from src/omnibot_manipulator/omnibot_manipulator_moveit/launch/sensor_manager.launch.xml
rename to src/omnibot_manipulator/omnibot_manipulator_moveit/launch/sensor_manager.launch
diff --git a/src/omnibot_platform/omnibot_platform_navigation/launch/rtabmap.launch b/src/omnibot_platform/omnibot_platform_navigation/launch/rtabmap.launch
index 008752c..f2f6438 100644
--- a/src/omnibot_platform/omnibot_platform_navigation/launch/rtabmap.launch
+++ b/src/omnibot_platform/omnibot_platform_navigation/launch/rtabmap.launch
@@ -40,14 +40,14 @@
         <!-- ################################################### -->
         <param name="RGBD/AngularUpdate"                        type="string"   value="0.1" /><!-- Update map only if the robot is moving -->
         <param name="RGBD/LinearUpdate"                         type="string"   value="0.1" /><!-- Update map only if the robot is moving -->
-        <param name="RGBD/OptimizeMaxError"                     type="double"   value="0"/> <!-- Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled-->
+        <param name="RGBD/OptimizeMaxError"                     type="double"   value="5"/> <!-- Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled-->
         <param name="RGBD/ProximityPathMaxNeighbors"            type="string"   value="0" />
         <param name="RGBD/CreateOccupancyGrid"                  type="string"   value="true" /> <!-- The proximity detection using only laser scans is disabled-->
         <param name="RGBD/NeighborLinkRefining"                 type="string"   value="true"/>  <!-- Do odometry correction with consecutive laser scans -->
         <param name="RGBD/ProximityBySpace"                     type="string"   value="true" /> <!-- Local loop closure detection (using estimated position) with locations in WM -->
         <param name="RGBD/ProximityByTime"                      type="string"   value="true"/> <!-- Local loop closure detection with locations in STM -->
         <param name="RGBD/OptimizeFromGraphEnd"                 type="string"   value="false" /><!-- Set to false to generate map correction between /map and /odom -->
-        <param name="RGBD/LocalRadius"                          type="string"   value="5"/>     <!-- limit length of proximity detections -->
+        <param name="RGBD/LocalRadius"                          type="string"   value="2"/>     <!-- limit length of proximity detections -->
         <!-- ################################################### -->
         <param name="Grid/MaxGroundHeight"                      type="string"   value="0.05"/>
         <param name="Grid/MaxObstacleHeight"                    type="string"   value="2.0"/>
@@ -67,15 +67,15 @@
         <param name="Mem/IncrementalMemory"                     type="string"   value="false"     if="$(arg localization)"/>
         <param name="Mem/IncrementalMemory"                     type="string"   value="true"     unless="$(arg localization)"/>
         <param name="Mem/InitWMWithAllNodes"                    type="string"   value="$(arg localization)"/>
-        <param name="Mem/STMSize"                               type="string"   value="30"/>    <!--  increased to 30 to avoid adding too many loop closures on just seen locations-->
+        <param name="Mem/STMSize"                               type="string"   value="40"/>    <!--  increased to 30 to avoid adding too many loop closures on just seen locations-->
         <!-- ################################################### -->
         <param name="Icp/CorrespondenceRatio"      type="string" value="0.3"/>
         <!-- ################################################### -->
-        <param name="Vis/MinInliers"                            type="string"   value="6" /><!-- 3D visual words minimum inliers to accept loop closure -->
-        <param name="Vis/InlierDistance"                        type="string"   value="0.1" /> <!-- 3D visual words correspondence distance -->
+        <param name="Vis/MinInliers"                            type="string"   value="15" /><!-- 3D visual words minimum inliers to accept loop closure -->
+        <param name="Vis/InlierDistance"                        type="string"   value="0.5" /> <!-- 3D visual words correspondence distance -->
         <!-- ################################################### -->
         <!-- ################################################### -->
-        <param name="Odom/Strategy"                             type="string"   value="0"/> <!-- 0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D-->
+        <param name="Odom/Strategy"                             type="string"   value="12"/> <!-- 0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D-->
         <!-- <param name="Odom/Holonomic"		                    ty/pe="string"   value="true"/> If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)). -->
         <param name="Odom/FilteringStrategy"                    type="string"   value="0"/> <!-- 0=No filtering 1=Kalman filtering 2=Particle filtering. This filter is used to smooth the odometry output.-->
 <!-- ################################################################################################################################################################# -->

From e1595abd9f2be4bc116dc2cb4e029dd8bd71e9ab Mon Sep 17 00:00:00 2001
From: IvanSnap360 <ivpetr2003@gmail.com>
Date: Sat, 10 Jun 2023 16:33:07 +0300
Subject: [PATCH 12/13] fix battery controller firmware

---
 .../battery_controller_firmware_V1.ino                   | 9 ++++-----
 firmware/battery_controller_firmware_V1/config.h         | 4 ++--
 2 files changed, 6 insertions(+), 7 deletions(-)

diff --git a/firmware/battery_controller_firmware_V1/battery_controller_firmware_V1.ino b/firmware/battery_controller_firmware_V1/battery_controller_firmware_V1.ino
index 54524a3..00fc212 100644
--- a/firmware/battery_controller_firmware_V1/battery_controller_firmware_V1.ino
+++ b/firmware/battery_controller_firmware_V1/battery_controller_firmware_V1.ino
@@ -25,12 +25,12 @@ void loop()
   if (millis() - send_lt > (uint32_t)(1000.0 / SERIAL_SEND_RATE_HZ))
   {
     battery[BATTERY_1][CELL_1] = getVoltsFromPin(battery_1_cell_pins[CELL_1]);
-    battery[BATTERY_1][CELL_2] = getVoltsFromPin(battery_1_cell_pins[CELL_2]);// - battery[BATTERY_1][CELL_1];
-    battery[BATTERY_1][CELL_3] = getVoltsFromPin(battery_1_cell_pins[CELL_3]);// - (battery[BATTERY_1][CELL_1] + battery[BATTERY_1][CELL_2]);
+    battery[BATTERY_1][CELL_2] = getVoltsFromPin(battery_1_cell_pins[CELL_2]) - battery[BATTERY_1][CELL_1];
+    battery[BATTERY_1][CELL_3] = getVoltsFromPin(battery_1_cell_pins[CELL_3]) - (battery[BATTERY_1][CELL_1] + battery[BATTERY_1][CELL_2]);
 
     battery[BATTERY_2][CELL_1] = getVoltsFromPin(battery_2_cell_pins[CELL_1]);
-    battery[BATTERY_2][CELL_2] = getVoltsFromPin(battery_2_cell_pins[CELL_2]);// - battery[BATTERY_2][CELL_1];
-    battery[BATTERY_2][CELL_3] = getVoltsFromPin(battery_2_cell_pins[CELL_3]);//- (battery[BATTERY_2][CELL_1] + battery[BATTERY_2][CELL_2]);
+    battery[BATTERY_2][CELL_2] = getVoltsFromPin(battery_2_cell_pins[CELL_2]) - battery[BATTERY_2][CELL_1];
+    battery[BATTERY_2][CELL_3] = getVoltsFromPin(battery_2_cell_pins[CELL_3]) - (battery[BATTERY_2][CELL_1] + battery[BATTERY_2][CELL_2]);
     print_values();
     send_lt = millis();
   }
@@ -67,7 +67,6 @@ float getVoltsFromPin(int pin)
 {
   float vin = 0.0;
   vin = ((analogRead(pin) * SOURCE_VOLTAGE_VOLTS) / ADC_MAX_VALUE) / (R2 / (R1 + R2));
-//  vin = analogRead(pin);/
   if (vin < 0.3)
     vin = 0.0;
 
diff --git a/firmware/battery_controller_firmware_V1/config.h b/firmware/battery_controller_firmware_V1/config.h
index 814a211..d876a18 100644
--- a/firmware/battery_controller_firmware_V1/config.h
+++ b/firmware/battery_controller_firmware_V1/config.h
@@ -33,9 +33,9 @@ enum
 
 
 #define RESITOR_DIVIDER__R1_VAL_Om (float)100000.0
-#define RESITOR_DIVIDER__R2_VAL_Om (float)10000.0
+#define RESITOR_DIVIDER__R2_VAL_Om (float)39000.0
 #define ADC_MAX_VALUE (float)1024.0
-#define SOURCE_VOLTAGE_VOLTS (float)4.6
+#define SOURCE_VOLTAGE_VOLTS (float)4.7
 
 const int battery_1_cell_pins[BATTERIES_SERIAL_CONFIGURATION] = {
     BATTERY_1_CELL_1_PIN,

From 2c268b864aa0230420635b6f0f1778a740e213d9 Mon Sep 17 00:00:00 2001
From: IvanSnap360 <ivpetr2003@gmail.com>
Date: Sat, 10 Jun 2023 21:34:51 +0300
Subject: [PATCH 13/13] add pointcloud relay node with control service

---
 src/omnibot_bringup/launch/omnibot_sim.launch | 49 +++++++-------
 src/omnibot_core/config/config.yaml           |  2 +-
 .../omnibot_core_lib/omnibot_core_lib.h       |  4 ++
 .../src/omnibot_core_lib/omnibot_core_lib.cpp | 21 +++++-
 .../launch/omnibot_manipulator_bringup.launch | 17 +++--
 .../CMakeLists.txt                            |  6 ++
 .../omnibot_manipulator_control_server_lib.h  |  2 +-
 .../launch/pointcloud_switcher.launch         | 10 +++
 .../src/pointcloud_swith_node.cpp             | 64 +++++++++++++++++++
 .../config/sensors_3d.yaml                    | 28 ++++----
 .../src/octomap_loader.py                     | 40 ------------
 11 files changed, 154 insertions(+), 89 deletions(-)
 create mode 100644 src/omnibot_manipulator/omnibot_manipulator_control/launch/pointcloud_switcher.launch
 create mode 100644 src/omnibot_manipulator/omnibot_manipulator_control/src/pointcloud_swith_node.cpp
 delete mode 100644 src/omnibot_manipulator/omnibot_manipulator_moveit/src/octomap_loader.py

diff --git a/src/omnibot_bringup/launch/omnibot_sim.launch b/src/omnibot_bringup/launch/omnibot_sim.launch
index eb9e565..1c45dc0 100644
--- a/src/omnibot_bringup/launch/omnibot_sim.launch
+++ b/src/omnibot_bringup/launch/omnibot_sim.launch
@@ -1,16 +1,16 @@
 <?xml version="1.0"?>
 <launch>
     <!--  -->
-    <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}] [${node}]: ${message}"/>
+    <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}] [${node}]: ${message}" />
 
     <!--  -->
     <arg name="use_manipulator" default="true" />
-    <arg name="rviz" default="true"/>
-    <arg name="rtabmap_viz" default="false"/>
-    
-    <arg name="localization" default="false"/>
-    
-    
+    <arg name="rviz" default="true" />
+    <arg name="rtabmap_viz" default="false" />
+
+    <arg name="localization" default="false" />
+
+
     <!--  -->
     <include file="$(find omnibot_description)/launch/load_description.launch">
         <arg name="gui" value="false" />
@@ -19,31 +19,30 @@
     </include>
 
     <!--  -->
-    <include file="$(find omnibot_control)/launch/omnibot_control.launch"/>
+    <include file="$(find omnibot_control)/launch/omnibot_control.launch" />
 
     <!--  -->
     <include file="$(find omnibot_platform_control)/launch/omnibot_platform_contol.launch">
-        <arg name="namespace" value="omnibot_robot"/>
+        <arg name="namespace" value="omnibot_robot" />
     </include>
 
     <!--  -->
-    <include file="$(find omnibot_platform_drv)/launch/omnibot_platform_drv.launch"/>
-   
+    <include file="$(find omnibot_platform_drv)/launch/omnibot_platform_drv.launch" />
+
     <!--  -->
-    <include file="$(find sensor_odom_manager)/launch/sensor_odom_manager.launch"/>
+    <include file="$(find sensor_odom_manager)/launch/sensor_odom_manager.launch" />
 
     <!--  -->
     <group if="$(arg use_manipulator)">
-        <include file="$(find omnibot_manipulator_control)/launch/omnibot_manipulator_control_server.launch"/>
-
-        <include file="$(find omnibot_manipulator_moveit)/launch/demo.launch">
+        <include
+            file="$(find omnibot_manipulator_bringup)/launch/omnibot_manipulator_bringup.launch">
+            <arg name="sim" value="true" />
             <arg name="use_rviz" value="false"/>
-            <arg name="sim" value="true"/>
         </include>
     </group>
 
 
-    <include file="$(find omnibot_core)/launch/omnibot_core.launch"/>
+    <include file="$(find omnibot_core)/launch/omnibot_core.launch" />
 
     <!--  -->
     <group if="$(arg rviz)">
@@ -55,20 +54,20 @@
     </group>
 
     <!--  -->
-    <include file="$(find omnibot_gazebo)/launch/omnibot_gazebo.launch"/>
+    <include file="$(find omnibot_gazebo)/launch/omnibot_gazebo.launch" />
 
     <!--  -->
     <group if="$(arg localization)">
         <!-- <include file="$(find omnibot_platform_navigation)/launch/load_map.launch"/>   -->
     </group>
-    
+
     <!--  -->
-    <include file="$(find omnibot_platform_navigation)/launch/omnibot_platform_navigation.launch" >
-        <arg name="sim"                     value="true"/>
-        <arg name="localization"            value="$(arg localization)"/>
-        <arg name="initial_pose"            value=""/>     
+    <include file="$(find omnibot_platform_navigation)/launch/omnibot_platform_navigation.launch">
+        <arg name="sim" value="true" />
+        <arg name="localization" value="$(arg localization)" />
+        <arg name="initial_pose" value="" />
     </include>
 
-<!-- START ROS WEB BRIDGE -->
-    <include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch"/>
+    <!-- START ROS WEB BRIDGE -->
+    <include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch" />
 </launch>
\ No newline at end of file
diff --git a/src/omnibot_core/config/config.yaml b/src/omnibot_core/config/config.yaml
index bde20db..ee1e2ec 100644
--- a/src/omnibot_core/config/config.yaml
+++ b/src/omnibot_core/config/config.yaml
@@ -1,4 +1,4 @@
 manipulator_on: true
 manipulator_cmd_service_name: manipulator_cmd_service
 gripper_cmd_service_name: gripper_cmd
-
+pointcloud_switcher_name: /pointcloud_swith_node/pointcloud_swith_node_cmd
diff --git a/src/omnibot_core/include/omnibot_core_lib/omnibot_core_lib.h b/src/omnibot_core/include/omnibot_core_lib/omnibot_core_lib.h
index 94cb7fd..15992a6 100644
--- a/src/omnibot_core/include/omnibot_core_lib/omnibot_core_lib.h
+++ b/src/omnibot_core/include/omnibot_core_lib/omnibot_core_lib.h
@@ -3,6 +3,7 @@
 #include <ros/ros.h>
 #include <omnibot_manipulator_control/manipulator_cmd.h>
 #include <omnibot_manipulator_control/gripper_cmd.h>
+#include <std_srvs/SetBool.h>
 #include <yaml-cpp/yaml.h>
 
 class OMNIBOT_CORE_LIB
@@ -17,6 +18,9 @@ class OMNIBOT_CORE_LIB
     std::string _gripper_service_name;
     ros::ServiceClient _gripper_service_client;
 
+    std::string _pointcloud_switcher_name;
+    ros::ServiceClient _pointcloud_switcher_client;
+
 public:
     OMNIBOT_CORE_LIB(ros::NodeHandle *nh, std::string config_path);
     ~OMNIBOT_CORE_LIB();
diff --git a/src/omnibot_core/src/omnibot_core_lib/omnibot_core_lib.cpp b/src/omnibot_core/src/omnibot_core_lib/omnibot_core_lib.cpp
index 44c21b0..156ba34 100644
--- a/src/omnibot_core/src/omnibot_core_lib/omnibot_core_lib.cpp
+++ b/src/omnibot_core/src/omnibot_core_lib/omnibot_core_lib.cpp
@@ -34,13 +34,20 @@ OMNIBOT_CORE_LIB::OMNIBOT_CORE_LIB(ros::NodeHandle *nh, std::string config_path)
         _gripper_service_client.waitForExistence();
         ROS_INFO("Waiting gripper_cmd service server...DONE");
 
+        _pointcloud_switcher_name = _cfg["pointcloud_switcher_name"].as<std::string>();
+        _pointcloud_switcher_client = nh->serviceClient<std_srvs::SetBool::Request,std_srvs::SetBool::Response>(_pointcloud_switcher_name);
+        ROS_INFO("Waiting _pointcloud_switcher service server...");
+        _pointcloud_switcher_client.waitForExistence();
+        ROS_INFO("Waiting _pointcloud_switche server...DONE");
+        
+
         ROS_INFO("Send manipulator to home pose...");
         omnibot_manipulator_control::manipulator_cmd::Request manip_requset;
         omnibot_manipulator_control::manipulator_cmd::Response manip_response;
         manip_requset.command_type = manip_requset.SAVED_POSE;
         manip_requset.command = "home";
         _manipulator_service_client.call(manip_requset, manip_response);
-        ROS_INFO("Send manipulator to home pose...%s", manip_response.result ? "DONE" : "FAILED");
+        ROS_INFO("Send manipulator to home pose...%s", manip_response.result ? "\033[32M DONE\033[0M" : "\033[31M FAILED\033[0M");
 
         ROS_INFO("Send gripper to home pose...");
         omnibot_manipulator_control::gripper_cmd::Request gripper_requset;
@@ -48,7 +55,17 @@ OMNIBOT_CORE_LIB::OMNIBOT_CORE_LIB(ros::NodeHandle *nh, std::string config_path)
         gripper_requset.command_type = gripper_requset.SAVED_POSE;
         gripper_requset.command = "open";
         _gripper_service_client.call(gripper_requset, gripper_response);
-        ROS_INFO("Send gripper to home pose...%s", gripper_response.result ? "DONE" : "FAILED");
+        ROS_INFO("Send gripper to home pose...%s", gripper_response.result ? "\033[32M DONE\033[0M" : "\033[31M FAILED\033[0M");
+
+
+        ROS_INFO("Enabling pointcloud....");
+        std_srvs::SetBool::Request pcs_req;
+        std_srvs::SetBool::Response pcs_res;
+        pcs_req.data = true;
+        _pointcloud_switcher_client.call(pcs_req,pcs_res);
+        ROS_INFO("Enabling pointcloud....%s", pcs_res.success ? "\033[32M DONE\033[0M" : "\033[31M FAILED\033[0M");
+
+
     }
 }
 
diff --git a/src/omnibot_manipulator/omnibot_manipulator_bringup/launch/omnibot_manipulator_bringup.launch b/src/omnibot_manipulator/omnibot_manipulator_bringup/launch/omnibot_manipulator_bringup.launch
index 8aa6b34..6d62703 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_bringup/launch/omnibot_manipulator_bringup.launch
+++ b/src/omnibot_manipulator/omnibot_manipulator_bringup/launch/omnibot_manipulator_bringup.launch
@@ -1,22 +1,27 @@
 <?xml version="1.0"?>
 <launch>
-    <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}] [${node}]: ${message}"/>
+    <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}] [${node}]: ${message}" />
 
     <arg name="sim" default="false" />
+    <arg name="use_rviz" default="false" />
     <include file="$(find omnibot_manipulator_moveit)/launch/demo.launch">
         <arg name="sim" value="$(arg sim)" />
-        <arg name="use_rviz" value="true" />
+        <arg name="use_rviz" value="$(arg use_rviz)" />
     </include>
 
 
-    <include
-        file="$(find omnibot_manipulator_hw_controller)/launch/omnibot_manipulator_hw_controller.launch" />
     <include
         file="$(find omnibot_manipulator_control)/launch/omnibot_manipulator_control_server.launch" />
-    <!-- <include file="$(find omnibot_manipulator_control)/launch/omnibot_manipulator_control.launch" /> -->
+    <group if="$(arg sim)">
+        <include
+            file="$(find omnibot_manipulator_control)/launch/omnibot_manipulator_control.launch" />
+    </group>
+
 
     <group unless="$(arg sim)">
+        <include
+            file="$(find omnibot_manipulator_hw_controller)/launch/omnibot_manipulator_hw_controller.launch" />
         <include file="$(find omnibot_manipulator_control)/launch/action_server.launch" />
     </group>
-
+    <include file="$(find omnibot_manipulator_control)/launch/pointcloud_switcher.launch" />
 </launch>
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/CMakeLists.txt b/src/omnibot_manipulator/omnibot_manipulator_control/CMakeLists.txt
index 5bdfdb6..abde369 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_control/CMakeLists.txt
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/CMakeLists.txt
@@ -178,6 +178,8 @@ add_executable(${PROJECT_NAME}_server_node
   src/action_server_lib/action_server_lib.cpp)
 
 add_executable(action_server_node src/action_server_node.cpp src/action_server_lib/action_server_lib.cpp)
+add_executable(pointcloud_swith_node src/pointcloud_swith_node.cpp)
+
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
 ## target back to the shorter version for ease of user use
@@ -200,6 +202,10 @@ target_link_libraries(action_server_node
 )
 
 
+target_link_libraries(pointcloud_swith_node
+  yaml-cpp
+  ${catkin_LIBRARIES}
+)
 #############
 ## Install ##
 #############
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/include/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.h b/src/omnibot_manipulator/omnibot_manipulator_control/include/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.h
index ceab3b1..080916e 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_control/include/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.h
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/include/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.h
@@ -42,7 +42,7 @@ class OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB
 
     ros::ServiceServer _arm_cmd_service_server;
     ros::ServiceServer _gripper_cmd_service_server;
-    
+
     bool ManipMoveByJointValues(std::vector<double> joint_values);
     bool ManipMoveByPosition(geometry_msgs::Pose new_pose);
     bool ManipMoveBySavedPosition(std::string pose_name);
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/launch/pointcloud_switcher.launch b/src/omnibot_manipulator/omnibot_manipulator_control/launch/pointcloud_switcher.launch
new file mode 100644
index 0000000..87947d3
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/launch/pointcloud_switcher.launch
@@ -0,0 +1,10 @@
+<?xml version="1.0"?>
+<launch>
+
+    <node pkg="omnibot_manipulator_control" type="pointcloud_swith_node" name="pointcloud_swith_node">
+        <param name="input_topic" value="/d400_arm/depth/color/points"/>
+        <param name="output_topic" value="/switcher/d400_arm/depth/color/points"/>
+        <param name="control_service_name" value="pointcloud_swith_node_cmd"/>
+    </node>
+
+</launch>
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/src/pointcloud_swith_node.cpp b/src/omnibot_manipulator/omnibot_manipulator_control/src/pointcloud_swith_node.cpp
new file mode 100644
index 0000000..0a5cd57
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/src/pointcloud_swith_node.cpp
@@ -0,0 +1,64 @@
+#include <ros/ros.h>
+#include "sensor_msgs/PointCloud2.h"
+#include "std_srvs/SetBool.h"
+ros::Subscriber input_topic_sub;
+ros::Publisher  output_topic_pub;
+ros::ServiceServer control_server;
+bool publish_enable = false;
+bool control_service_cb_f(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
+{
+    publish_enable = (bool)req.data;
+    res.message = "Pointcloud publisher is " + req.data ? "ENABLED" : "DISABLED";
+    res.success = true;
+    return true;
+}
+
+void input_topic_sub_cb_f(const sensor_msgs::PointCloud2::ConstPtr &in_data)
+{
+    if (publish_enable)
+    {
+        output_topic_pub.publish(in_data);
+    }
+}
+
+int main(int argc, char *argv[])
+{
+    ros::init(argc, argv, "pointcloud_swith_node");
+    ros::NodeHandle nh("~");
+
+    std::string input_topic;
+    std::string output_topic;
+    std::string control_service_name;
+
+    if (ros::param::has("~input_topic"))
+    {
+        ros::param::get("~input_topic", input_topic);
+    }
+    else
+    {
+        ROS_ERROR("Param 'input_topic' is not set ");
+    }
+    if (ros::param::has("~output_topic"))
+    {
+        ros::param::get("~output_topic", output_topic);
+    }
+    else
+    {
+        ROS_ERROR("Param 'output_topic' is not set ");
+    }
+    if (ros::param::has("~control_service_name"))
+    {
+        ros::param::get("~control_service_name", control_service_name);
+    }
+    else
+    {
+        ROS_ERROR("Param 'control_service_name' is not set ");
+    }
+
+    input_topic_sub = nh.subscribe<sensor_msgs::PointCloud2>(input_topic, 10, input_topic_sub_cb_f);
+    output_topic_pub = nh.advertise<sensor_msgs::PointCloud2>(output_topic, 10);
+    control_server  = nh.advertiseService(control_service_name,control_service_cb_f);
+    ros::spin();
+    ros::shutdown();
+    return 0;
+}
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/config/sensors_3d.yaml b/src/omnibot_manipulator/omnibot_manipulator_moveit/config/sensors_3d.yaml
index 7bbd230..81d9e3e 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/config/sensors_3d.yaml
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/config/sensors_3d.yaml
@@ -1,18 +1,18 @@
-sensors:
-  []
+# sensors:
+#   []
 
-# sensors: 
-#   - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
-#     # point_cloud_topic: /rtabmap/cloud_map
-#     # point_cloud_topic: /rtabmap/cloud_obstacles
-#     # point_cloud_topic: /rtabmap/local_grid_obstacle
-#     point_cloud_topic: /d400_arm/depth/color/points
-#     max_range: 2.0
-#     max_update_rate: 100.0
-#     point_subsample: 1
-#     padding_offset: 0.08
-#     padding_scale: 1.0
-#     filtered_cloud_topic: filtered_cloud
+sensors: 
+  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
+    # point_cloud_topic: /rtabmap/cloud_map
+    # point_cloud_topic: /rtabmap/cloud_obstacles
+    # point_cloud_topic: /rtabmap/local_grid_obstacle
+    point_cloud_topic: /switcher/d400_arm/depth/color/points
+    max_range: 3.0
+    max_update_rate: 50.0
+    point_subsample: 1
+    padding_offset: 0.08
+    padding_scale: 1.0
+    filtered_cloud_topic: filtered_cloud
 
   # - sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
   #   image_topic: /d400_arm/aligned_depth_to_color/image_raw
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/src/octomap_loader.py b/src/omnibot_manipulator/omnibot_manipulator_moveit/src/octomap_loader.py
deleted file mode 100644
index 87bebfb..0000000
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/src/octomap_loader.py
+++ /dev/null
@@ -1,40 +0,0 @@
-#! /usr/bin/env python3
-
-import rospy
-from octomap_msgs.msg import Octomap
-from moveit_msgs.msg import PlanningScene, PlanningSceneWorld
-
-class OctoHandler():
-    mapMsg = None
-
-    def __init__(self):
-        rospy.init_node('moveit_octomap_handler')
-        rospy.Subscriber('/rtabmap/octomap_full', Octomap, self.cb, queue_size=1)
-        pub = rospy.Publisher('/move_group/monitored_planning_scene', PlanningScene, queue_size=1)
-        pub2 = rospy.Publisher('/planning_scene', PlanningScene, queue_size=1)
-        r = rospy.Rate(10)
-        while(not rospy.is_shutdown()):
-            if(self.mapMsg is not None):
-                pub.publish(self.mapMsg)
-                pub2.publish(self.mapMsg)
-            else:
-                pass
-            r.sleep()
-
-    def cb(self, msg):
-        print("recieve cloud_map")
-        psw = PlanningSceneWorld()
-        psw.octomap.header.stamp = rospy.Time.now()
-        psw.octomap.header.frame_id = 'map'
-        psw.octomap.octomap = msg
-
-        psw.octomap.origin.position.x = 0
-        psw.octomap.origin.orientation.w = 1
-
-        ps = PlanningScene()
-        ps.world = psw
-        ps.is_diff = True
-        self.mapMsg = ps
-
-if __name__ == '__main__':
-    octomap_object = OctoHandler()
\ No newline at end of file