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(self, wait_time=0):
        self.msg.mode = 11
        self.msg.gait_id = 26
        self.msg.vel_des = [0, 0, 0]
        self.msg.duration = 10
        self.msg.life_count += 1
        self.ctrl.Send_cmd(self.msg)
        if wait_time:
            time.sleep(wait_time)
        
    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)
        
        """
        专为旋转动作设计的平滑停止方法，通过更精细的步骤减小速度
        
        参数:
        current_vel: 当前速度列表 [x, y, w]，如果为None则使用当前消息中的速度
        steps: 减速步骤数，默认为5步（比普通停止更多步骤）
        delay: 每步之间的延迟时间(秒)，默认为0.1秒（更快的响应）
        final_steps: 最后完全停止前的额外零速度指令次数，增强稳定性
        """
        
        # 如果没有提供当前速度，使用消息中的当前速度
        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
            
        # 1. 先针对角速度进行单独减速，确保旋转先停稳
        if len(current_vel) >= 3 and abs(current_vel[2]) > 0.05:  # 角速度
            # 首先只减小角速度，保持线速度
            for i in range(1, steps + 1):
                # 计算这一步的减速比例 - 角速度减速更快
                ratio = (steps - i) / steps
                ratio_w = ratio * ratio  # 角速度使用平方递减，减速更快
                
                # 计算新的速度，只改变角速度
                new_vel = current_vel.copy()
                new_vel[2] = current_vel[2] * ratio_w
                
                # 更新消息
                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)
        
        # 2. 再减小整体速度
        current_vel_no_w = current_vel.copy()
        if len(current_vel) >= 3:
            current_vel_no_w[2] = 0  # 角速度已减为0
            
        for i in range(1, steps + 1):
            # 计算这一步的减速比例
            ratio = (steps - i) / steps
            
            # 计算新的速度
            new_vel = [v * ratio for v in current_vel_no_w]
            if len(current_vel) >= 3:
                new_vel[2] = 0  # 确保角速度为0
            
            # 更新消息
            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)
        
        # 3. 发送多次零速度命令，确保完全停止
        for _ in range(final_steps):
            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)