（12-3-02）基于全向驱动的机器人及其控制系统：机器人运动控制和状态管理（2）

4.3.4  升级的运动控制逻辑

文件controller_2.py实现一个 ROS 2 节点，该节点控制机器人沿特定轨迹移动，并处理机器人之间的避碰和速度调整。文件controller_2.py通过计算机器人 1 和机器人 3 之间的距离来判断是否需要发布避碰速度。如果机器人距离过近，则发布默认速度；否则，计算目标点的速度并根据逆运动学计算每个轮子的速度，发布适当的速度指令。代码还处理了笔的状态和路径点的更新，并在路径完成后调用重置服务。

和前面的文件controller_1.py相比，文件controller_2.py升级了运动控制逻辑：增加了路径点的定义、笔状态的处理以及更复杂的控制逻辑，适用于需要沿特定路径移动并控制多个机器人和笔状态的应用场景。文件controller_2.py的具体实现流程如下所示。

（1）通过如下代码生成路径点并初始化相关变量，通过 np.linspace 函数生成从的 50 个均匀分布的角度值 t1。基于这些角度计算路径上的 x 和 y 坐标，并将其存储在 x1 和 y1 中。随后，将一些控制相关的变量（如 bot1_index、bot2_index、stop1_flag 等）初始化为 0，以便在后续的控制逻辑中使用。

t1 = np.linspace((2*np.pi)/3, (4*np.pi)/3, 50)
x1 = (220*np.cos(4*t1)*np.cos(t1)) + (250 - 2.5)
y1 = (-220*np.cos(4*t1)*np.sin(t1)) + (250 - 25.45) + 15

# 将变量初始化为0
bot1_index = 0
bot2_index = 0
bot3_index = 0
bot1_prev_index = 0
bot2_prev_index = 0
bot3_prev_index = 0
stop1_flag = 0
stop2_flag = 0
stop3_flag = 0
last_err_x = 0
last_err_y = 0

（2）定义类Controller，该类继承自 ROS 2 的 Node。它负责创建多个订阅者来接收机器人的位置和笔状态，并通过发布者发布机器人速度和笔的状态。类Controller中各个成员方法的功能如下：

1. 方法 bot1_callback(self, msg)：功能是更新机器人1的位置信息（pose1_x、pose1_y 和 pose1_theta），根据传入的消息 msg 中的坐标和角度值。
2. 方法 bot2_callback(self, msg)：功能是更新机器人2的位置信息（pose2_x、pose2_y 和 pose2_theta），根据传入的消息 msg 中的坐标和角度值。
3. 方法 bot3_callback(self, msg)：功能是更新机器人3的位置信息（pose3_x、pose3_y 和 pose3_theta），根据传入的消息 msg 中的坐标和角度值。
4. 方法 pen1down_cb(self, data)：功能是更新机器人1的笔状态（pen1），根据传入的数据 data 中的布尔值来设置笔是否在下方。
5. 方法 pen2down_cb(self, data)：功能是更新机器人2的笔状态（pen2），根据传入的数据 data 中的布尔值来设置笔是否在下方。
6. 方法 pen3down_cb(self, data)：功能是更新机器人3的笔状态（pen3），根据传入的数据 data 中的布尔值来设置笔是否在下方。
7. 方法 reset_serv(self)：功能是调用一个名为 /Stop_Flag 的重置服务，进行系统重启或清除操作。它首先检查服务是否可用，然后发送一个空的请求以触发服务。

class Controller(Node):
    def __init__(self):
        super().__init__('Controller')
        self.pen1_subcriber = self.create_subscription(Pose2D, "/pen22_pose", self.bot1_callback, 10)
        self.pen2_subcriber = self.create_subscription(Pose2D, "/pen11_pose", self.bot2_callback, 10)
        self.pen3_subcriber = self.create_subscription(Pose2D, "/pen33_pose", self.bot3_callback, 10)

        self.bot1_pen_sub = self.create_subscription(Bool, "/pen1_down", self.pen1down_cb, 10)
        self.bot2_pen_sub = self.create_subscription(Bool, "/pen2_down", self.pen2down_cb, 10)
        self.bot3_pen_sub = self.create_subscription(Bool, "/pen3_down", self.pen3down_cb, 10)

        self.bot1_pub = self.create_publisher(Twist, "/bot2_vel", 10)
        self.bot1_pen_pub = self.create_publisher(Bool, "/pen2_down", 10)

        self.pose1_theta = 0
        self.pose1_x = 0
        self.pose1_y = 0
        self.pose3_theta = 0
        self.pose3_x = 0
        self.pose3_y = 0
        self.pose2_theta = 0
        self.pose2_x = 0
        self.pose2_y = 0
        self.bot1_flag = 0
        self.bot2_flag = 0
        self.bot3_flag = 0
        self.pen1 = 0
        self.pen2 = 0
        self.pen3 = 0

    # 定义回调函数
    def bot1_callback(self, msg):
        self.pose1_x = msg.x
        self.pose1_y = msg.y
        self.pose1_theta = msg.theta

    def bot2_callback(self, msg):
        self.pose2_x = msg.x
        self.pose2_y = msg.y
        self.pose2_theta = msg.theta

    def bot3_callback(self, msg):
        self.pose3_x = msg.x
        self.pose3_y = msg.y
        self.pose3_theta = msg.theta

    def pen1down_cb(self, data):
        self.pen1 = data.data

    def pen2down_cb(self, data):
        self.pen2 = data.data

    def pen3down_cb(self, data):
        self.pen3 = data.data

    # 定义一个函数来调用重置服务
    def reset_serv(self):
       client2 = self.create_client(Empty, "/Stop_Flag")
       while not client2.wait_for_service(1.0):
            self.get_logger().warn("等待服务中")
       request = Empty.Request()
       client2.call_async(request)

（3）函数 rot_pts 的功能是将给定的坐标点 (x, y) 依据指定的角度 angle 进行旋转，该函数将角度从度数转换为弧度，并利用旋转矩阵计算旋转后的坐标点，最后返回旋转后的 x 和 y 坐标。

def rot_pts(x,y,angle):
        angle_r = (angle*math.pi)/180
        c = np.cos(angle_r)
        s = np.sin(angle_r)
        rot_mat = np.array([[c, s], [-s, c]])
        pts_to_be_rot = np.array([[x], [y]])
        pts_rotated = np.dot(rot_mat, pts_to_be_rot)
        rotated_x = (pts_rotated[0])[0]
        rotated_y = (pts_rotated[1])[0]
        return rotated_x, rotated_y

（4）函数 inverse_kinematics 的功能是计算每个轮子的速度，以实现机器人的逆运动学。该函数接受旋转误差速度（包括线速度和角速度）作为输入，将这些速度与特定于机器人的计算矩阵相乘，从而得到逆运动学速度。计算中使用了一个常量 r 和一个矩阵 cal_mat，最终返回每个轮子的速度。

def inverse_kinematics(rotated_x, rotated_y, theta_r):
        r = 4.318 
        d = 25.00 
        theta =  (theta_r*math.pi)/180
        vel = np.array([[theta],
                        [rotated_x],
                        [-rotated_y]])
        cal_mat = np.array([[-d, 1.0, 0],
                            [-d, -0.5, -np.sin(np.pi/3) + 0.0],
                            [-d, -0.5, np.sin(np.pi/3) + 0.0]])
                
        inv_vel = (np.dot(cal_mat, vel))/r
        return inv_vel

（5）定义函数 main，这是 ROS 2 节点的主循环函数。首先初始化 ROS 2 环境并创建 Controller 类的实例。接着进入无限循环，计算机器人与目标点之间的距离，并根据距离发布适当的速度指令以避免碰撞或调整机器人的移动。函数还处理机器人的笔状态，并在目标点附近进行必要的更新或重置操作。每次循环结束时调用 rclpy.spin_once 处理回调，最终在程序结束时销毁节点并关闭 ROS 2 环境。

def main(args=None):
    rclpy.init(args=args)
    # 创建 Controller 类的实例
    con = Controller()
    # 主循环
    while (1):
         # 计算机器人 2 和机器人 1 之间的距离
        cx2 = con.pose2_x - con.pose1_x
        cy2 = con.pose2_y - con.pose1_y
        co_dist_2 = math.sqrt(cx2**2 + cy2**2)
        # 避免碰撞的算法
        if co_dist_2 < 110:
            bot1 = Twist()
            bot1.linear.x = 90.0
            bot1.linear.y = 90.0
            bot1.linear.z = 90.0
            con.bot1_pub.publish(bot1)
        else:
            # 初始化常量
            kd = 0.00006
            kp_ang = 1.5
            global bot1_index, bot1_prev_index, bot1_index, stop1_flag, stop2_flag, stop3_flag, last_err_x, last_err_y
            goal_x = (x1[bot1_index])
            goal_y = (y1[bot1_index])
            goal_theta = 0
            pose1_theta = con.pose1_theta
            # 获取机器人的朝向
            if pose1_theta > 180:
                pose1_theta = pose1_theta - 360
         
            vel_x = goal_x - con.pose1_x
            vel_y = goal_y - con.pose1_y
            err_theta = (goal_theta - pose1_theta)
            err_x = goal_x - con.pose1_x
            err_y = goal_y - con.pose1_y
            diff_x = err_x - last_err_x
            diff_y = err_y - last_err_y

            new_vel_x = (rot_pts(vel_x, vel_y, pose1_theta))[0] +kd*diff_x
            new_vel_y = (rot_pts(vel_x, vel_y, pose1_theta))[1] +kd*diff_y
            # 获取逆运动学速度
            v11 = (((inverse_kinematics(new_vel_x, new_vel_y, -(err_theta)*kp_ang)[0])[0]))
            v22 = (((inverse_kinematics(new_vel_x, new_vel_y, -(err_theta)*kp_ang)[1])[0]))
            v33 = (((inverse_kinematics(new_vel_x, new_vel_y, -(err_theta)*kp_ang)[2])[0]))
            v1 = v11*1.5 + 90
            v2 = v22*1.5 + 90
            v3 = v33*1.5 + 90 
            print (v1, v2, v3)

            # 根据条件发布适当的机器人速度
            distance = math.sqrt(vel_x**2 + vel_y**2) 
            stop_threshold = 20.0
            if distance > stop_threshold:
                bot1 = Twist()
                bot1.linear.x = float(v1)
                bot1.linear.y = float(v2)
                bot1.linear.z = float(v3)
                con.bot1_pub.publish(bot1)
            else:
                bot1 = Twist()
                bot1.linear.x = 90.0
                bot1.linear.y = 90.0
                bot1.linear.z = 90.0
                con.bot1_pub.publish(bot1)

            if bot1_prev_index == 0 and bot1_index == 0:
                bul = Bool()
                bul.data = False
                con.bot1_pen_pub.publish(bul)
                
            else:
                bul = Bool()
                bul.data = True
                con.bot1_pen_pub.publish(bul)
                

            bot1_prev_index = bot1_index

            if distance < stop_threshold and con.pen1 and con.pen2 and con.pen3:
                if bot1_index < (len(x1)-1):
                    bot1_index += 1
                elif bot1_index == (len(x1)-1):
                    bot1 = Twist()
                    bot1.linear.x = 90.0
                    bot1.linear.y = 90.0
                    bot1.linear.z = 90.0
                    con.bot1_pub.publish(bot1)
                    stop1_flag = 1
                    con.reset_serv()
                else:
                    bot1_index = 0
            elif distance < stop_threshold and bot1_index == 0:
                bot1_index += 1      
            print("x1_index:", bot1_index)
            last_err_x = err_x
            last_err_y = err_y
        rclpy.spin_once(con)
    con.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

上述代码的实现流程如下：

1. 初始化和创建实例：通过rclpy.init(args=args) 初始化 ROS 2，通过con = Controller() 创建 Controller 类的实例。
2. 主循环：在无限循环中计算机器人 2 和机器人 1 之间的距离 co_dist_2。如果距离小于 110，发布一个速度为 90 的 Twist 消息，表示机器人应该停止或缓慢移动。
3. 计算目标速度：如果距离大于 110，执行以下操作：

• 初始化常量 kd 和 kp_ang。
• 计算目标位置 goal_x 和 goal_y，以及机器人当前的朝向 pose1_theta。
• 计算误差值 err_x、err_y 和 err_theta，并通过 rot_pts 函数旋转点。
• 计算新的速度 new_vel_x 和 new_vel_y，使用 inverse_kinematics 函数计算每个轮子的速度 v1、v2 和 v3。
• 根据距离发布合适的速度。如果距离大于阈值 stop_threshold，则发布计算得到的速度；否则，发布默认速度 90。

1. 控制标志和笔状态：根据条件发布 Bool 消息来控制机器人的笔状态，并检查距离是否小于 stop_threshold，并且所有笔状态都为 True，然后更新 bot1_index，或者重置服务。
2. 更新索引和错误值：根据 bot1_index 和距离来更新机器人的目标点，更新上一次的错误值 last_err_x 和 last_err_y。
3. 处理回调和关闭节点：使用 rclpy.spin_once(con) 处理回调函数，在循环结束后，销毁节点并关闭 ROS。

4.4  调试运行