mi-task/utils/base_msg.py

import time

class BaseMsg:
    def __init__(self, ctrl, msg):
        self.ctrl = ctrl
        self.msg = msg

    def stop_force(self):
        self.msg.mode = 0
        self.msg.gait_id = 0
        self.msg.duration = 0
        self.msg.life_count += 1
        self.ctrl.Send_cmd(self.msg)
        self.ctrl.Wait_finish(0, 0)
        
    def stop_smooth(self, current_vel=None, steps=3, delay=0.2):
        """
        平滑停止移动，通过逐步减小速度而不是强制停止
        
        参数:
        current_vel: 当前速度列表 [x, y, z]，如果为None则使用当前消息中的速度
        steps: 减速步骤数，默认为3步
        delay: 每步之间的延迟时间(秒)，默认为0.2秒
        """
        
        # 如果没有提供当前速度，使用消息中的当前速度
        if current_vel is None:
            current_vel = self.msg.vel_des.copy() if hasattr(self.msg, 'vel_des') else [0, 0, 0]
        
        # 保存当前消息的一些参数
        current_mode = self.msg.mode
        current_gait_id = self.msg.gait_id
        
        # 检查是否有速度需要减小
        if all(abs(v) < 0.01 for v in current_vel):
            # 速度已经很小，直接停止
            self.stop_force()
            return
            
        # 逐步减小速度
        for i in range(1, steps + 1):
            # 计算这一步的减速比例
            ratio = (steps - i) / steps
            
            # 计算新的速度
            new_vel = [v * ratio for v in current_vel]
            
            # 更新消息
            self.msg.mode = current_mode
            self.msg.gait_id = current_gait_id
            self.msg.vel_des = new_vel
            self.msg.duration = int(delay * 1000)  # 毫秒
            self.msg.life_count += 1
            
            # 发送命令
            self.ctrl.Send_cmd(self.msg)
            
            # 等待这一步完成
            time.sleep(delay)
        
        # 最后发送一个零速度命令，确保完全停止
        self.msg.mode = current_mode
        self.msg.gait_id = current_gait_id
        self.msg.vel_des = [0, 0, 0]
        self.msg.duration = int(delay * 1000)
        self.msg.life_count += 1
        self.ctrl.Send_cmd(self.msg)
        
        # 等待最后的命令完成
        time.sleep(delay)