diff --git a/src/lite6_controller/CMakeLists.txt b/src/lite6_controller/CMakeLists.txt
index 5c26af6..05bf62b 100644
--- a/src/lite6_controller/CMakeLists.txt
+++ b/src/lite6_controller/CMakeLists.txt
@@ -16,6 +16,8 @@ find_package(moveit REQUIRED)
 find_package(pilz_industrial_motion_planner REQUIRED)
 find_package(moveit_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(xarm_api REQUIRED)
+find_package(xarm_msgs REQUIRED)
 
 find_package(tf2_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
@@ -45,6 +47,18 @@ ament_target_dependencies(lite6_controller
   "moveit_ros_planning_interface"
   "robot_interfaces")
 
+add_executable(lite6_calibration src/lite6_calibration.cpp)
+ament_target_dependencies(lite6_calibration
+  "xarm_msgs"
+  "rclcpp"
+  "moveit"
+  "rclcpp_action"
+  "Eigen3"
+  "pilz_industrial_motion_planner"
+  "robot_controller"
+  "moveit_ros_planning_interface"
+  "robot_interfaces")
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
@@ -52,6 +66,7 @@ endif()
 
 install(TARGETS
   lite6_controller
+  lite6_calibration
   DESTINATION lib/${PROJECT_NAME})
 
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
diff --git a/src/lite6_controller/launch/lite6_real_calibration.launch.py b/src/lite6_controller/launch/lite6_real_calibration.launch.py
new file mode 100644
index 0000000..a680e9f
--- /dev/null
+++ b/src/lite6_controller/launch/lite6_real_calibration.launch.py
@@ -0,0 +1,126 @@
+import os
+from launch import LaunchDescription
+from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
+
+from ament_index_python import get_package_share_directory
+from launch.launch_description_sources import load_python_launch_file_as_module
+from launch import LaunchDescription
+from launch.actions import RegisterEventHandler, EmitEvent
+from launch.event_handlers import OnProcessExit
+from launch.events import Shutdown
+
+
+def launch_setup(context, *args, **kwargs):
+    robot_ip = LaunchConfiguration('robot_ip', default='192.168.1.150')
+    report_type = LaunchConfiguration('report_type', default='normal')
+    prefix = LaunchConfiguration('prefix', default='')
+    hw_ns = LaunchConfiguration('hw_ns', default='ufactory')
+    limited = LaunchConfiguration('limited', default=True)
+    effort_control = LaunchConfiguration('effort_control', default=False)
+    velocity_control = LaunchConfiguration('velocity_control', default=False)
+    add_gripper = LaunchConfiguration('add_gripper', default=False)
+    add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False)
+    dof = LaunchConfiguration('dof', default=6)
+    robot_type = LaunchConfiguration('robot_type', default='lite')
+    no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False)
+
+    add_other_geometry = LaunchConfiguration('add_other_geometry', default=False)
+    geometry_type = LaunchConfiguration('geometry_type', default='box')
+    geometry_mass = LaunchConfiguration('geometry_mass', default=0.1)
+    geometry_height = LaunchConfiguration('geometry_height', default=0.1)
+    geometry_radius = LaunchConfiguration('geometry_radius', default=0.1)
+    geometry_length = LaunchConfiguration('geometry_length', default=0.1)
+    geometry_width = LaunchConfiguration('geometry_width', default=0.1)
+    geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='')
+    geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"')
+    geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"')
+    geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"')
+    geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"')
+
+    baud_checkset = LaunchConfiguration('baud_checkset', default=True)
+    default_gripper_baud = LaunchConfiguration('default_gripper_baud', default=2000000)
+
+    ros2_control_plugin = 'uf_robot_hardware/UFRobotSystemHardware'
+    controllers_name = LaunchConfiguration('controllers_name', default='controllers')
+    moveit_controller_manager_key = LaunchConfiguration('moveit_controller_manager_key', default='moveit_simple_controller_manager')
+    moveit_controller_manager_value = LaunchConfiguration('moveit_controller_manager_value', default='moveit_simple_controller_manager/MoveItSimpleControllerManager')
+
+    xarm_type = '{}{}'.format(robot_type.perform(context), dof.perform(context))
+    ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context)
+
+    use_sim_time = LaunchConfiguration('use_sim_time', default=False)
+    log_level = LaunchConfiguration("log_level", default='warn')
+
+    # robot_description_parameters
+    # xarm_moveit_config/launch/lib/robot_moveit_config_lib.py
+    moveit_config_package_name = 'custom_xarm_moveit_config'
+    mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory(moveit_config_package_name), 'launch', 'lib', 'robot_moveit_config_lib.py'))
+    get_xarm_robot_description_parameters = getattr(mod, 'get_xarm_robot_description_parameters')
+    robot_description_parameters = get_xarm_robot_description_parameters(
+        xacro_urdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
+        xacro_srdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
+        urdf_arguments={
+            'prefix': prefix,
+            'hw_ns': hw_ns.perform(context).strip('/'),
+            'limited': limited,
+            'effort_control': effort_control,
+            'velocity_control': velocity_control,
+            'add_gripper': add_gripper,
+            'add_vacuum_gripper': add_vacuum_gripper,
+            'dof': dof,
+            'robot_type': robot_type,
+            'ros2_control_plugin': ros2_control_plugin,
+            'add_other_geometry': add_other_geometry,
+            'geometry_type': geometry_type,
+            'geometry_mass': geometry_mass,
+            'geometry_height': geometry_height,
+            'geometry_radius': geometry_radius,
+            'geometry_length': geometry_length,
+            'geometry_width': geometry_width,
+            'geometry_mesh_filename': geometry_mesh_filename,
+            'geometry_mesh_origin_xyz': geometry_mesh_origin_xyz,
+            'geometry_mesh_origin_rpy': geometry_mesh_origin_rpy,
+            'geometry_mesh_tcp_xyz': geometry_mesh_tcp_xyz,
+            'geometry_mesh_tcp_rpy': geometry_mesh_tcp_rpy,
+        },
+        srdf_arguments={
+            'prefix': prefix,
+            'dof': dof,
+            'robot_type': robot_type,
+            'add_gripper': add_gripper,
+            'add_vacuum_gripper': add_vacuum_gripper,
+            'add_other_geometry': add_other_geometry,
+        },
+        arguments={
+            'context': context,
+            'xarm_type': xarm_type,
+        }
+    )
+    calibration_node = Node(
+            package="lite6_controller",
+            executable="lite6_calibration",
+            output="screen",
+            arguments=["--ros-args", "--log-level", log_level],
+            parameters=[
+                robot_description_parameters,
+                {"use_sim_time": use_sim_time},
+            ],
+        )
+
+    return [
+        RegisterEventHandler(event_handler=OnProcessExit(
+            target_action=calibration_node,
+            on_exit=[EmitEvent(event=Shutdown())]
+        )),
+        calibration_node
+    ]
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        OpaqueFunction(function=launch_setup)
+    ])
diff --git a/src/lite6_controller/package.xml b/src/lite6_controller/package.xml
index 8b8d793..de24f32 100644
--- a/src/lite6_controller/package.xml
+++ b/src/lite6_controller/package.xml
@@ -15,6 +15,7 @@
   <!-- <depend>robot_controller</depend> -->
   <depend>moveit</depend>
   <depend>moveit_msgs</depend>
+  <depend>xarm_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>moveit_ros_planning_interface</depend>
   <depend>tf2_ros</depend>
diff --git a/src/lite6_controller/src/lite6_calibration.cpp b/src/lite6_controller/src/lite6_calibration.cpp
new file mode 100644
index 0000000..40d77c7
--- /dev/null
+++ b/src/lite6_controller/src/lite6_calibration.cpp
@@ -0,0 +1,128 @@
+#include <cstdio>
+#include <rclcpp/rclcpp.hpp>
+#include <robot_controller/robot_controller.hpp>
+#include <chrono>
+#include <cstdlib>
+#include <queue>
+
+#include <fstream>
+#include <iostream>
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <moveit_msgs/msg/collision_object.hpp>
+
+//#include <moveit/planning_interface/planning_interface.h>
+//#include <moveit/planning_interface/planning_response.h>
+//#include <moveit/kinematic_constraints/kinematic_constraint.h>
+#include <moveit/kinematic_constraints/utils.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <tf2_eigen/tf2_eigen.hpp>
+
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/common_planning_interface_objects/common_objects.h>
+#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
+
+
+#include <moveit_msgs/msg/constraints.hpp>
+#include <moveit_msgs/msg/robot_trajectory.hpp>
+#include <moveit_msgs/msg/motion_plan_request.hpp>
+#include <moveit_msgs/msg/motion_sequence_request.hpp>
+#include <moveit_msgs/msg/motion_sequence_item.hpp>
+#include <moveit_msgs/srv/get_motion_sequence.hpp>
+
+
+#include <moveit/moveit_cpp/moveit_cpp.h>
+#include <moveit/moveit_cpp/planning_component.h>
+
+#include <moveit/robot_trajectory/robot_trajectory.h>
+
+#include <pilz_industrial_motion_planner/command_list_manager.h>
+
+#include <xarm_msgs/srv/move_joint.hpp>
+#include <xarm_msgs/srv/get_float32_list.hpp>
+
+const std::string MOVE_GROUP = "lite6";
+
+using namespace std::chrono_literals;
+
+class Calibration : public rclcpp::Node
+{
+  public:
+    //moveit::planning_interface::MoveGroupInterface move_group;
+
+    rclcpp::TimerBase::SharedPtr status_timer;
+
+    rclcpp::Client<xarm_msgs::srv::MoveJoint>::SharedPtr set_joint_client;
+    rclcpp::Client<xarm_msgs::srv::GetFloat32List>::SharedPtr get_joint_client;
+
+    Calibration(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
+      : Node("lite6_calibration",options)
+        //move_group(std::shared_ptr<rclcpp::Node>(std::move(this)), MOVE_GROUP)
+    {
+      //RCLCPP_INFO(this->get_logger(), "Starting state monitor");
+      //move_group.startStateMonitor(30);
+      //RCLCPP_INFO(this->get_logger(), "Started state monitor");
+      //move_group.setPlanningTime(30.0);
+
+      //status_timer = this->create_wall_timer(5s, std::bind(&Calibration::getTree, this));
+
+      RCLCPP_ERROR(this->get_logger(), "create service client");
+      set_joint_client = this->create_client<xarm_msgs::srv::MoveJoint>("/ufactory/set_servo_angle");
+      get_joint_client = this->create_client<xarm_msgs::srv::GetFloat32List>("/ufactory/get_servo_angle");
+      //xarm_msgs::srv::MoveJoint
+      while (!set_joint_client->wait_for_service(1s)) {
+        RCLCPP_ERROR(this->get_logger(), "service not available, waiting again...");
+      }
+      while (!get_joint_client->wait_for_service(1s)) {
+        RCLCPP_ERROR(this->get_logger(), "service not available, waiting again...");
+      }
+      RCLCPP_ERROR(this->get_logger(), "Client ready");
+
+      //RCLCPP_ERROR(this->get_logger(), "create request");
+      //auto request = std::make_shared<xarm_msgs::srv::MoveJoint::Request>();
+      //RCLCPP_ERROR(this->get_logger(), "send request");
+      //auto result = joint_client->async_send_request(request);
+      //RCLCPP_ERROR(this->get_logger(), "get request");
+      //auto a = result.get()->message;
+      //RCLCPP_ERROR(this->get_logger(), "A:%s", a.c_str());
+
+      auto request = std::make_shared<xarm_msgs::srv::GetFloat32List::Request>();
+      RCLCPP_ERROR(this->get_logger(), "");
+      auto result = get_joint_client->async_send_request(request);
+      auto msg = result.get()->message;
+      RCLCPP_ERROR(this->get_logger(), "A:%s", msg.c_str());
+
+    }
+
+    void getTree()
+    {
+      //rclcpp::sleep_for(20s);
+      RCLCPP_INFO(this->get_logger(), "Getting robot state");
+      //moveit::core::RobotStatePtr move_group_state  = move_group.getCurrentState(30);
+      //RCLCPP_INFO(this->get_logger(), move_group_state->getStateTreeString().c_str());
+      //RCLCPP_ERROR(this->get_logger(), "AAAAAAAAAA");
+    }
+
+};
+
+/**
+ *
+ */
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+
+
+  auto calibration = std::make_shared<Calibration>();
+
+  rclcpp::executors::MultiThreadedExecutor executor;
+  executor.add_node(calibration);
+  executor.spin();
+
+  rclcpp::shutdown();
+  return EXIT_SUCCESS;
+}