pymycobot 3.5.0.dev4__tar.gz → 3.5.0.dev6__tar.gz

{pymycobot-3.5.0.dev4/pymycobot.egg-info → pymycobot-3.5.0.dev6}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: pymycobot
-Version: 3.5.0.dev4
+Version: 3.5.0.dev6
 Summary: Python API for serial communication of MyCobot.
 Home-page: https://github.com/elephantrobotics/pymycobot
 Author: Elephantrobotics

{pymycobot-3.5.0.dev4 → pymycobot-3.5.0.dev6}/pymycobot/__init__.py

@@ -23,7 +23,7 @@ from pymycobot.elephantrobot import ElephantRobot
 from pymycobot.mercury import Mercury
 from pymycobot.myagv import MyAgv
 from pymycobot.mecharmsocket import MechArmSocket
-from pymycobot.mercurychassis import MercuryChassis
+from pymycobot.mercurychassis import ChassisControl
 from pymycobot.mercurysocket import MercurySocket
 from pymycobot.mycobotpro630 import Phoenix
 from pymycobot.myarmm import MyArmM
@@ -49,7 +49,7 @@ __all__ = [
     "MyAgv",
     "MechArmSocket",
     "MyArmSocket",
-    "MercuryChassis",
+    "ChassisControl",
     "MercurySocket",
     "Phoenix",
     "MyArmM",
@@ -61,7 +61,7 @@ if sys.platform == "linux":
     from pymycobot.mybuddyemoticon import MyBuddyEmoticon
     __all__.append("MyBuddyEmoticon")
 
-__version__ = "3.5.0dev4" 
+__version__ = "3.5.0dev6" 
 __author__ = "Elephantrobotics"
 __email__ = "weiquan.xu@elephantrobotics.com"
 __git_url__ = "https://github.com/elephantrobotics/pymycobot"

{pymycobot-3.5.0.dev4 → pymycobot-3.5.0.dev6}/pymycobot/common.py

@@ -22,9 +22,15 @@ class ProtocolCode(object):
     CLEAR_ERROR_INFO = 0x08
     GET_ATOM_VERSION = 0x09
     
+    CLEAR_ZERO_POS = 0x0A
+    SET_SERVO_CW = 0x0B
+    GET_SERVO_CW = 0x0C
+    CLEAR_WAIST_QUEUE = 0x0D
+    
     SetHTSGripperTorque = 0x35
     GetHTSGripperTorque = 0x36
     GetGripperProtectCurrent = 0x37
+    JOG_INCREMENT_BASE_COORD = 0x37
     InitGripper = 0x38
     SetGripperProtectCurrent = 0x39
 

{pymycobot-3.5.0.dev4 → pymycobot-3.5.0.dev6}/pymycobot/error.py

@@ -238,15 +238,15 @@ def calibration_parameters(**kwargs):
     robot_limit = {
         "Mercury": {
             "id": [1, 2, 3, 4, 5, 6, 11, 12, 13],
-            "angles_min": [-165, -55, -180, -165, -20, -180, -60, -140, -120],
-            "angles_max": [165, 95, 5, 165, 273, 180, 0, 190, 120],
+            "angles_min": [-165, -55, -173, -165, -20, -180, -60, -138, -118],
+            "angles_max": [165, 95, 5, 165, 265, 180, 0, 188, 118],
             "coords_min": [-441.37, -441.37, -206.52, -180, -180, -180],
             "coords_max": [441.37, 441.37, 628.02, 180, 180, 180]
         },
         "MercurySocket": {
             "id": [1, 2, 3, 4, 5, 6, 11, 12, 13],
-            "angles_min": [-165, -55, -180, -165, -20, -180, -60, -140, -120],
-            "angles_max": [165, 95, 5, 165, 273, 180, 0, 190, 120],
+            "angles_min": [-165, -55, -173, -165, -20, -180, -60, -138, -118],
+            "angles_max": [165, 95, 5, 165, 265, 180, 0, 188, 118],
             "coords_min": [-441.37, -441.37, -206.52, -180, -180, -180],
             "coords_max": [441.37, 441.37, 628.02, 180, 180, 180]
         },

{pymycobot-3.5.0.dev4 → pymycobot-3.5.0.dev6}/pymycobot/mercury_api.py

@@ -61,21 +61,21 @@ class MercuryCommandGenerator(CommandGenerator):
             elif genre in [ProtocolCode.POWER_OFF, ProtocolCode.RELEASE_ALL_SERVOS, ProtocolCode.FOCUS_ALL_SERVOS,
                            ProtocolCode.RELEASE_SERVO, ProtocolCode.FOCUS_SERVO, ProtocolCode.STOP]:
                 wait_time = 3
-            elif genre in [
-                ProtocolCode.SEND_ANGLE,
-                ProtocolCode.SEND_ANGLES,
-                ProtocolCode.SEND_COORD,
-                ProtocolCode.SEND_COORDS,
-                ProtocolCode.JOG_ANGLE,
-                ProtocolCode.JOG_COORD,
-                ProtocolCode.JOG_INCREMENT,
-                ProtocolCode.JOG_INCREMENT_COORD,
-                ProtocolCode.COBOTX_SET_SOLUTION_ANGLES,
-                ProtocolCode.MERCURY_SET_BASE_COORDS,
-                ProtocolCode.MERCURY_JOG_BASE_COORD,
-                ProtocolCode.MERCURY_SET_BASE_COORD]:
-                wait_time = 300
-                is_in_position = True
+            # elif genre in [
+            #     ProtocolCode.SEND_ANGLE,
+            #     ProtocolCode.SEND_ANGLES,
+            #     ProtocolCode.SEND_COORD,
+            #     ProtocolCode.SEND_COORDS,
+            #     ProtocolCode.JOG_ANGLE,
+            #     ProtocolCode.JOG_COORD,
+            #     ProtocolCode.JOG_INCREMENT,
+            #     ProtocolCode.JOG_INCREMENT_COORD,
+            #     ProtocolCode.COBOTX_SET_SOLUTION_ANGLES,
+            #     ProtocolCode.MERCURY_SET_BASE_COORDS,
+            #     ProtocolCode.MERCURY_JOG_BASE_COORD,
+            #     ProtocolCode.MERCURY_SET_BASE_COORD]:
+            #     wait_time = 300
+            #     is_in_position = True
             need_break = False
             data = None
             while True and time.time() - t < wait_time:
@@ -181,18 +181,18 @@ class MercuryCommandGenerator(CommandGenerator):
                         one = valid_data[i : i + 2]
                         res.append(self._decode_int16(one))
                         i+=2
-            # elif data_len == 38:
-            #     # right arm get_robot_status
-            #     i = 0
-            #     res = []
-            #     while i < data_len:
-            #         if i < 10 or i >= 30:
-            #             res.append(valid_data[i])
-            #             i+=1
-            #         elif i < 38:
-            #             one = valid_data[i : i + 2]
-            #             res.append(self._decode_int16(one))
-            #             i+=2
+            elif data_len == 30:
+                # right arm get_robot_status
+                i = 0
+                res = []
+                while i < data_len:
+                    if i < 8 or i >= 24:
+                        res.append(valid_data[i])
+                        i+=1
+                    elif i < 23:
+                        one = valid_data[i : i + 2]
+                        res.append(self._decode_int16(one))
+                        i+=2
             elif data_len == 48:
                 # get_pos_over
                 for i in range(0, data_len, 8):
@@ -297,7 +297,7 @@ class MercuryCommandGenerator(CommandGenerator):
                             if res[i] == 1:
                                 r.append(i)
                 return r
-            elif genre in [ProtocolCode.COBOTX_GET_ANGLE, ProtocolCode.COBOTX_GET_SOLUTION_ANGLES, ProtocolCode.MERCURY_GET_POS_OVER_SHOOT]:
+            elif genre in [ProtocolCode.COBOTX_GET_ANGLE, ProtocolCode.COBOTX_GET_SOLUTION_ANGLES, ProtocolCode.MERCURY_GET_POS_OVER_SHOOT, ProtocolCode.GET_SERVO_CW]:
                     return self._int2angle(res[0])
             elif genre == ProtocolCode.MERCURY_ROBOT_STATUS:
                 if len(res) == 23:
@@ -437,8 +437,9 @@ class MercuryCommandGenerator(CommandGenerator):
                         for d in datas:
                             command_log += hex(d)[2:] + " "
                         self.log.debug("_read : {}".format(command_log))
-                    with self.lock:
-                        self.read_command.append(res)
+                    if res != []:
+                        with self.lock:
+                            self.read_command.append(res)
                 # return datas
         
     def set_solution_angles(self, angle, speed):
@@ -1022,4 +1023,43 @@ class MercuryCommandGenerator(CommandGenerator):
                 5 : Drag teaching sampling period
             value (int): Filter length, range is 1 ~ 100
         """
-        return self._mesg(ProtocolCode.SET_FILTER_LEN, rank, value)
+        return self._mesg(ProtocolCode.SET_FILTER_LEN, rank, value)
+    
+    def clear_zero_pos(self):
+        return self._mesg(ProtocolCode.CLEAR_ZERO_POS)
+    
+    def set_servo_cw(self, joint_id, err_angle):
+        """Set the joint in-place feedback error angle
+
+        Args:
+            joint_id (int): Joint ID, 11 or 12.
+            err_angle (float): Error range is 0 ~ 5.
+        """
+        return self._mesg(ProtocolCode.SET_SERVO_CW, joint_id, [self._angle2int(err_angle)])
+    
+    def get_servo_cw(self, joint_id):
+        """Get the joint in-place feedback error angle
+
+        Args:
+            joint_id (int): Joint ID, 11 or 12.
+
+        Returns:
+            float: Error angle, range is 0 ~ 5.
+        """
+        return self._mesg(ProtocolCode.GET_SERVO_CW, joint_id, has_reply=True)
+    
+    def clearWaistQueue(self):
+        """Clear the cache points of the three motors in the torso
+        """
+        return self._mesg(ProtocolCode.CLEAR_WAIST_QUEUE)
+    
+    def jog_increment_base_coord(self, axis, increment, speed):
+        """Base coordinate stepping control
+
+        Args:
+            axis (int): axis id, range 1 ~ 6 corresponds to [x,y,z,rx,ry,rz]
+            increment (float): Incremental value
+            speed (int): speed
+        """
+        return self._mesg(ProtocolCode.JOG_INCREMENT_BASE_COORD, axis, [self._angle2int(increment)], speed)
+    

pymycobot-3.5.0.dev6/pymycobot/mercurychassis.py

@@ -0,0 +1,325 @@
+#!/usr/bin/python
+# -*- coding:utf-8 -*-
+# @File    : mercurychassis_api.py
+# @Author  : Wang Weijian
+# @Time    :  2024/05/31 15:19:47
+# @function: the script is used to do something
+# @version : V1
+import traceback
+from datetime import datetime
+import time
+import serial
+
+
+class ProtocolCode(object):
+    # send data
+    header = 0x7B
+    footer = 0x7D
+    # Ultrasonic data
+    ultrasound_header = 0xFA
+    ultrasound_footer = 0xFC
+
+
+class ChassisControl:
+
+    def __init__(self, port="/dev/wheeltec_controller", baudrate=115200, timeout=0.1, debug=False):
+        """
+        Args:
+            port     : port string
+            baudrate : baud rate string, default '115200'
+            timeout  : default 0.1
+            debug    : whether show debug info, default: False
+        """
+
+        self._serial_port = serial.Serial()
+        self._serial_port.port = port
+        self._serial_port.baudrate = baudrate
+        self._serial_port.timeout = timeout
+        self._serial_port.rts = True
+        self._serial_port.dtr = True
+        self._serial_port.open()
+        self.debug = debug
+        self.Send_Data = [0] * 11
+        time.sleep(0.2)
+
+    def _write(self):
+        pass
+
+    def _read(self):
+        data = b""
+        t = time.time()
+        time.sleep(0.1)
+        if self._serial_port.inWaiting() > 0:
+            data = self._serial_port.read(self._serial_port.inWaiting())
+        return data
+
+    def _debug(self, data):
+        """whether show info."""
+        hex_data = " ".join(f"{value:02X}" for value in data)
+        current_time = datetime.now().strftime("%Y-%m-%d %H:%M:%S")
+        if self.debug:
+            print(f"\n***** Debug Info *****\n{current_time} send command: {hex_data}")
+
+    def close(self):
+        self._serial_port.close()
+
+    def open(self):
+        self._serial_port.open()
+
+    def _request(self, flag=""):
+        """
+        Read Data
+        :param flag: Data type parameter variable
+        :return:
+        """
+        receive_all_data = self._read()
+        receive_all_data = [byte for byte in receive_all_data]
+        return_data = ''
+        if flag == "voltage":
+            receive_data = self._extract_frame(receive_all_data, ProtocolCode.header, ProtocolCode.footer)
+            # print(receive_data, len(receive_data))
+            self._debug(receive_data)
+            transition_16 = 0
+            transition_16 |= receive_data[20] << 8
+            transition_16 |= receive_data[21]
+            return_data = round(transition_16 / 1000 + (transition_16 % 1000) * 0.001, 3)
+
+            return return_data
+
+        elif flag == "ultrasonic":
+            receive_data = self._extract_frame(receive_all_data, ProtocolCode.ultrasound_header,
+                                              ProtocolCode.ultrasound_footer)
+            # print(receive_data, len(receive_data))
+            self._debug(receive_data)
+            return_data = [receive_data[1] * 256 + receive_data[2], receive_data[3] * 256 + receive_data[4],
+                           receive_data[5] * 256 + receive_data[6]]
+            return return_data
+        else:
+            print('no data', return_data)
+
+    def _extract_frame(self, data, frame_header, frame_tail):
+        """
+        Extract the corresponding data
+        :param data: Get all data
+        :param frame_header: Frame Header
+        :param frame_tail: Frame tail
+        :return: A list of integers
+        """
+        try:
+            # Find the location of the frame header
+            start_index = data.index(frame_header)
+            # Find the end of the frame, starting from the frame header
+            end_index = data.index(frame_tail, start_index)
+            # Extracting dataframe
+            frame = data[start_index:end_index + 1]
+            return frame
+        except ValueError:
+            # If the frame header or frame trailer is not found, an empty list is returned.
+            return []
+
+    def _check_sum(self, count_number, mode):
+        """
+        Calculate the check digit
+        :param count_number: The total number of bytes before the check digit
+        :param mode: Verify whether to send or receive data, 0-Receive data mode, 1-Send data mode
+        :return: A byte type
+        """
+        check_sum_result = 0
+        # 接收数据模式
+        if mode == 0:
+            for k in range(count_number):
+                check_sum_result ^= self.Send_Data[k]
+        # 发送数据模式
+        elif mode == 1:
+            for k in range(count_number):
+                check_sum_result ^= self.Send_Data[k]
+
+        return check_sum_result
+
+    def get_power_voltage(self):
+        """
+        Get battery voltage
+        :return:
+        """
+        return self._request("voltage")
+
+    def get_ultrasonic_value(self):
+        """
+        Get ultrasonic value
+        :return:
+        """
+        return self._request('ultrasonic')
+
+    def go_straight(self, speed=0.2):
+        """
+        Forward control
+        :param speed: speed (float, optional): Movement speed. Defaults to 0.2. range 0 ~ 1
+        :return:
+        """
+        if speed < 0 or speed > 1:
+            raise Exception("The movement speed range is 0~1, but the received value is {}".format(speed))
+        self.Send_Data[0] = ProtocolCode.header
+        self.Send_Data[1] = 0
+        self.Send_Data[2] = 0
+        # The target velocity of the X-axis of the robot
+        transition = int(speed * 1000)
+        self.Send_Data[4] = transition & 0xFF  # Lower 8 bits
+        self.Send_Data[3] = (transition >> 8) & 0xFF  # Higher 8 bits
+        # The target velocity of the Y-axis of the robot
+        self.Send_Data[6] = 0
+        self.Send_Data[5] = 0
+
+        self.Send_Data[8] = 0
+        self.Send_Data[7] = 0
+        # Check the bits for the Check_Sum function
+        self.Send_Data[9] = self._check_sum(9, 1)
+        self.Send_Data[10] = ProtocolCode.footer
+
+        try:
+            self._serial_port.write(bytes(self.Send_Data))
+            self._serial_port.flush()
+            self._debug(self.Send_Data)
+        except serial.SerialException as e:
+            e = traceback.format_exc()
+            print('Unable to send data through serial port: {}'.format(e))
+
+    def go_back(self, speed=-0.2):
+        """
+        Back control
+        :param speed: speed (float, optional): Movement speed. Defaults to 0.25. range -1 ~ 0
+        :return:
+        """
+        if not -1 <= speed <= 0:
+            raise Exception("The movement speed range is -1~0, but the received value is {}".format(speed))
+        self.Send_Data = [0] * 11
+        self.Send_Data[0] = ProtocolCode.header
+        self.Send_Data[1] = 0
+        self.Send_Data[2] = 0
+        # The target velocity of the X-axis of the robot
+        transition = int(speed * 1000)
+        if transition < 0:
+            transition = (1 << 16) + transition
+        self.Send_Data[4] = transition & 0xFF  # Lower 8 bits
+        self.Send_Data[3] = (transition >> 8) & 0xFF  # Higher 8 bits
+        # The target velocity of the Y-axis of the robot
+        self.Send_Data[6] = 0
+        self.Send_Data[5] = 0
+        # The target velocity of the Z-axis of the robot
+        self.Send_Data[8] = 0
+        self.Send_Data[7] = 0
+        # Check the bits for the Check_Sum function
+        self.Send_Data[9] = self._check_sum(9, 1)
+        self.Send_Data[10] = ProtocolCode.footer
+
+        try:
+            self._serial_port.write(bytes(self.Send_Data))
+            self._serial_port.flush()
+            self._debug(self.Send_Data)
+        except serial.SerialException as e:
+            e = traceback.format_exc()
+            print('Unable to send data through serial port: {}'.format(e))
+
+    def turn_left(self, speed=0.2):
+        """
+        Left turn control
+        :param speed: speed (float, optional): Movement speed. Defaults to 0.2. range 0 ~ 1
+        :return:
+        """
+
+        if speed < 0 or speed > 1:
+            raise Exception("The movement speed range is 0~1, but the received value is {}".format(speed))
+        self.Send_Data = [0] * 11
+        self.Send_Data[0] = ProtocolCode.header
+        self.Send_Data[1] = 0
+        self.Send_Data[2] = 0
+        # The target velocity of the X-axis of the robot
+        self.Send_Data[4] = 0
+        self.Send_Data[3] = 0
+        # The target velocity of the Y-axis of the robot
+        self.Send_Data[6] = 0
+        self.Send_Data[5] = 0
+        # The target velocity of the Z-axis of the robot
+        transition = int(speed * 1000)
+        self.Send_Data[8] = transition & 0xFF  # Lower 8 bits
+        self.Send_Data[7] = (transition >> 8) & 0xFF  # Higher 8 bits
+        # Check the bits for the Check_Sum function
+        self.Send_Data[9] = self._check_sum(9, 1)
+        self.Send_Data[10] = ProtocolCode.footer
+
+        try:
+            self._serial_port.write(bytes(self.Send_Data))
+            self._serial_port.flush()
+            self._debug(self.Send_Data)
+        except serial.SerialException as e:
+            e = traceback.format_exc()
+            print('Unable to send data through serial port: {}'.format(e))
+
+    def turn_right(self, speed=-0.2):
+        """
+        Right turn control
+        :param speed: speed (float, optional): Movement speed. Defaults to -0.2. range -1 ~ 0
+        :return:
+        """
+        if not -1 <= speed <= 0:
+            raise Exception("The movement speed range is -1~0, but the received value is {}".format(speed))
+        self.Send_Data = [0] * 11
+
+        self.Send_Data[0] = ProtocolCode.header
+        self.Send_Data[1] = 0
+        self.Send_Data[2] = 0
+        # The target velocity of the X-axis of the robot
+        self.Send_Data[4] = 0
+        self.Send_Data[3] = 0
+        # The target velocity of the Y-axis of the robot
+        self.Send_Data[6] = 0
+        self.Send_Data[5] = 0
+        # The target velocity of the Z-axis of the robot
+        transition = int(speed * 1000)
+        if transition < 0:
+            transition = (1 << 16) + transition
+        self.Send_Data[8] = transition & 0xFF  # Lower 8 bits
+        self.Send_Data[7] = (transition >> 8) & 0xFF  # Higher 8 bits
+        # Check the bits for the Check_Sum function
+        self.Send_Data[9] = self._check_sum(9, 1)
+        self.Send_Data[10] = ProtocolCode.footer
+
+        try:
+            self._serial_port.write(bytes(self.Send_Data))
+            self._serial_port.flush()
+            self._debug(self.Send_Data)
+        except serial.SerialException as e:
+            e = traceback.format_exc()
+            print('Unable to send data through serial port: {}'.format(e))
+
+    def stop(self):
+        """
+        stop motion
+        :return:
+        """
+        self.Send_Data = [0] * 11
+        self.Send_Data[0] = ProtocolCode.header
+        self.Send_Data[1] = 0
+        self.Send_Data[2] = 0
+        # The target velocity of the X-axis of the robot
+        self.Send_Data[4] = 0
+        self.Send_Data[3] = 0
+        # The target velocity of the Y-axis of the robot
+        self.Send_Data[6] = 0
+        self.Send_Data[5] = 0
+        # The target velocity of the Z-axis of the robot
+        self.Send_Data[8] = 0
+        self.Send_Data[7] = 0
+        # Check the bits for the Check_Sum function
+        self.Send_Data[9] = self._check_sum(9, 1)
+        self.Send_Data[10] = ProtocolCode.footer
+
+        try:
+            self._serial_port.write(bytes(self.Send_Data))
+            self._serial_port.flush()
+            self._debug(self.Send_Data)
+        except serial.SerialException as e:
+            e = traceback.format_exc()
+            print('Unable to send data through serial port: {}'.format(e))
+
+
+# mc = ChassisControl('/dev/wheeltec_controller', debug=True)

{pymycobot-3.5.0.dev4 → pymycobot-3.5.0.dev6/pymycobot.egg-info}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: pymycobot
-Version: 3.5.0.dev4
+Version: 3.5.0.dev6
 Summary: Python API for serial communication of MyCobot.
 Home-page: https://github.com/elephantrobotics/pymycobot
 Author: Elephantrobotics

pymycobot-3.5.0.dev4/pymycobot/mercurychassis.py

@@ -1,170 +0,0 @@
-
-import socket
-import json
-import threading
-import struct
-
-class MercuryChassisError(Exception):
-    pass
-
-class MercuryChassis:
-    """
-    Mercury X1 mobile chassis car socket class
-    """
-    def __init__(self, ip=None):
-        self.ifname = b"wlan0"
-        self._sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
-        if ip is not None:
-            self.host = ip
-        else:
-            import fcntl
-            try:
-                self.host = socket.inet_ntoa(fcntl.ioctl(self.server_socket.fileno(), 0x8915, struct.pack('256s', self.ifname[:15]))[20:24])  #IP
-            except:
-                self.host = "127.0.0.1"
-        self._sock.connect((self.host, 9999))
-        self.recv = threading.Thread(target=self.check_move_end, daemon=True)
-        self.recv.start()
-        self.move_end = False
-        
-    def close(self):
-        self._sock.close()
-    
-    def open(self):
-        self._sock.connect((self.host, 9999))
-        
-    def check_move_end(self):
-        while True:
-            try:
-                data = self._sock.recv(1024)
-                data = json.loads(data)
-                self.move_end = data
-            except:
-                pass
-    
-    # @property
-    def is_move_end(self):
-        """Is the movement over
-
-        Returns:
-            _type_: _description_
-        """
-        return self.move_end
-        
-    def go_straight(self, speed=0.25, exercise_duration=5):
-        """Forward control
-
-        Args:
-            speed (float, optional): Movement speed. Defaults to 0.25. range 0 ~ 1
-            exercise_duration (int, optional): Exercise duration. Defaults to 5s.
-        """
-        if speed < 0 or speed > 1:
-            raise MercuryChassisError("The movement speed range is 0~1, but the received value is {}".format(speed))
-        command = {"goStraight": {"time": exercise_duration, "speed": speed}}
-        self._sock.sendall(json.dumps(command).encode())
-        
-    def go_back(self, speed=0.25, exercise_duration=5):
-        """Back control
-
-        Args:
-            speed (float, optional): Movement speed. Defaults to 0.25. range 0 ~ 1
-            exercise_duration (int, optional): Exercise duration. Defaults to 5s.
-        """
-        if speed < 0 or speed > 1:
-            raise MercuryChassisError("The movement speed range is 0~1, but the received value is {}".format(speed))
-        command = {"goBack": {"time": exercise_duration, "speed": speed}}
-        self._sock.sendall(json.dumps(command).encode())
-        
-    def turn_left(self, speed=0.5, exercise_duration=5):
-        """left turn control
-
-        Args:
-            speed (float, optional): Movement speed. Defaults to 0.25. range 0 ~ 1
-            exercise_duration (int, optional): Exercise duration. Defaults to 5s.
-        """
-        if speed < 0 or speed > 1:
-            raise MercuryChassisError("The movement speed range is 0~1, but the received value is {}".format(speed))
-        command = {"turnLeft": {"time": exercise_duration, "speed": speed}}
-        self._sock.sendall(json.dumps(command).encode())
-        
-    def turn_right(self, speed=0.5, exercise_duration=5):
-        """_summary_
-
-        Args:
-            speed (float, optional): Movement speed. Defaults to 0.25. range 0 ~ 1
-            exercise_duration (int, optional): Exercise duration. Defaults to 5s.
-        """
-        if speed < 0 or speed > 1:
-            raise MercuryChassisError("The movement speed range is 0~1, but the received value is {}".format(speed))
-        command = {"turnRight": {"time": exercise_duration, "speed": speed}}
-        self._sock.sendall(json.dumps(command).encode())
-        
-    def stop(self):
-        """stop motion"""
-        command = {"stop": True}
-        self._sock.sendall(json.dumps(command).encode())
-
-    def init_position(self,position_x,position_y,orientation_z,orientation_w,covariance):
-        """Set navigation starting position
-
-        Args:
-            position_x (_type_): _description_
-            position_y (_type_): _description_
-            orientation_z (_type_): _description_
-            orientation_w (_type_): _description_
-            covariance (_type_): _description_
-        """
-        command = {"initPosition": {"x": position_x, "y": position_y, "o_z": orientation_z, "o_w": orientation_w, "cov": covariance}}
-        self._sock.sendall(json.dumps(command).encode())
-
-    def goto_position(self,position_x,position_y,orientation_z,orientation_w):
-        """Set navigation target location
-
-        Args:
-            position_x (_type_): _description_
-            position_y (_type_): _description_
-            orientation_z (_type_): _description_
-            orientation_w (_type_): _description_
-        """
-        command = {"goToPosition": {"x": position_x, "y": position_y, "o_z": orientation_z, "o_w": orientation_w}}
-        self._sock.sendall(json.dumps(command).encode())
-
-    def cancel_navigation(self):
-        """Cancel navigation
-        """
-        command = {"movebaseCancel": True}
-        self._sock.sendall(json.dumps(command).encode())        
-
-    def get_software_version(self):
-        """Get the base server version number"""
-        command = {"getSoftWareVersion": True}
-        self._sock.sendall(json.dumps(command).encode()) 
-        while True:
-            if self.move_end:
-                data = self.move_end.get("getSoftWareVersion", None)
-                if data:
-                    return self.move_end["getSoftWareVersion"]["return"]
-
-    def get_base_ros_version(self):
-        """Get the base ROS project version number"""
-        command = {"getBaseROSVersion": True}
-        self._sock.sendall(json.dumps(command).encode()) 
-        while True:
-            if self.move_end:
-                data = self.move_end.get("getBaseROSVersion", None)
-                if data:
-                    return self.move_end["getBaseROSVersion"]["return"]
-
-    def get_battery_state(self):
-        """Get battery level
-        """
-        command = {"batteryState": True}
-        self._sock.sendall(json.dumps(command).encode())
-        while True:
-            if self.move_end:
-                data = self.move_end.get("batteryState", None)
-                if data:
-                    return self.move_end["batteryState"]["return"]
-        
-
-