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

4.3  机器人运动控制和状态管理

在本项目的“task6_ws”目录中包含了实现一个综合的机器人控制系统的程序文件，负责机器人的运动控制、状态管理和位置估计。包括控制机器人的移动和反馈信息的管理，使用标记识别技术来定位和调整机器人位置。

4.3.1  启动文件

文件task6_controller.launch.py定义了一个 ROS2 启动文件，用于启动三个控制节点。每个节点对应一个不同的控制器，分别运行 controller_1、controller_2 和 controller_3 可执行文件，所有这些可执行文件都属于同一个名为 task_6 的 ROS2 包。

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    ld = LaunchDescription()
    controller1_node = Node(
        package="task_6",           # 输入你的ROS2包的名称
        executable="controller_1",  # 输入你的可执行文件的名称
    )
    controller2_node = Node(
        package="task_6",           # 输入你的ROS2包的名称
        executable="controller_2",  # 输入你的可执行文件的名称
    )
    controller3_node = Node(
        package="task_6",           # 输入你的ROS2包的名称
        executable="controller_3",  # 输入你的可执行文件的名称
    )
    ld.add_action(controller1_node)
    ld.add_action(controller2_node)
    ld.add_action(controller3_node)

    return ld

4.3.2  生成环境

使用ROS2工具生成脚本文件_local_setup_util_sh.py，功能是生成设置ROS2包环境的Shell命令，并按拓扑顺序处理包的依赖关系。此文件可以处理不同类型的环境变量设置，并输出在Shell中使用的命令。脚本支持追加、前置和设置环境变量等操作，并确保这些操作遵循包之间的依赖关系，从而正确配置包的执行环境。

4.3.3  机器人运动控制

文件controller_1.py的功能是控制机器人在指定轨迹上移动并避免碰撞的ROS 2节点。在控制过程中接收来自机器人位置的消息，根据计算的逆运动学速度控制机器人的运动，同时避免与其他机器人发生碰撞，并通过发布速度和笔的状态来控制机器人的行为。

文件controller_1.py的具体实现流程如下所示。

（1）下面的代码用于生成一条轨迹，并初始化了一些全局变量以用于跟踪和控制三个机器人的状态。轨迹通过计算特定范围内的数学函数生成，接着这些全局变量将被用于控制机器人的运动、检测碰撞以及管理机器人的停止状态。

# 生成轨迹点
t1 = np.linspace((0*np.pi)/3, (2*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

# 全局变量初始化
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 节点类，用于控制和管理三个机器人。通过订阅机器人和笔的位置及状态话题，Controller能够接收机器人的位置信息和笔的状态，并发布相应的速度指令以控制机器人移动。

class Controller(Node):
    def __init__(self):
        super().__init__('Controller')
        self.pen1_subcriber = self.create_subscription(Pose2D, "/pen11_pose", self.bot1_callback, 10)
        self.pen2_subcriber = self.create_subscription(Pose2D, "/pen22_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, "/bot1_vel", 10)
        self.bot1_pen_pub = self.create_publisher(Bool, "/pen1_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.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)

在类Controller中包含如下所示的成员方法：

1. __init__(self)：功能是初始化 Controller 节点。该方法创建了多个订阅者和发布者，用于处理机器人位置、笔状态的更新和发布。它还初始化了一些用于存储机器人位置、角度和笔状态的变量。
2. bot1_callback(self, msg) ：功能是处理第一个机器人的位置更新。该方法将接收到的位置信息更新到存储第一个机器人位置和角度的变量（pose1_x, pose1_y, pose1_theta）中。
3. bot2_callback(self, msg) ：功能是处理第二个机器人的位置更新。该方法将接收到的位置信息更新到存储第二个机器人位置和角度的变量（pose2_x, pose2_y, pose2_theta）中。
4. bot3_callback(self, msg) ：功能是处理第三个机器人的位置更新。该方法将接收到的位置信息更新到存储第三个机器人位置和角度的变量（pose3_x, pose3_y, pose3_theta）中。
5. pen1down_cb(self, data) ：功能是处理第一个机器人的笔状态更新。该方法将接收到的笔状态更新到存储第一个机器人笔状态的变量（pen1）中。
6. pen2down_cb(self, data) ：功能是处理第二个机器人的笔状态更新。该方法将接收到的笔状态更新到存储第二个机器人笔状态的变量（pen2）中。
7. pen3down_cb(self, data) ：功能是处理第三个机器人的笔状态更新。该方法将接收到的笔状态更新到存储第三个机器人笔状态的变量（pen3）中。
8. reset_serv(self) ：功能是调用服务以重置机器人的状态。该方法创建一个 Empty 服务客户端，等待服务可用后，发送请求以调用 /Stop_Flag 服务。

（3）函数rot_pts的功能是将给定的坐标点 (x, y) 旋转指定角度后返回新的坐标。该函数通过将角度从度转换为弧度，利用旋转矩阵对点进行旋转计算，从而得到旋转后的坐标 (rotated_x, rotated_y)。

# 定义一个函数来旋转点
'''
* 函数名称：rot_pts()
* 输入：要旋转的坐标
* 输出：旋转后的坐标
'''
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的功能是根据给定的旋转误差速度（包括线速度和角速度）计算每个轮子的速度。该函数通过将速度矩阵与特定于机器人的计算矩阵相乘，并进行缩放，来得到每个轮子的逆运动学速度。

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 节点并创建 Controller 类的实例，然后进入主循环。在主循环中，首先计算机器人 1 和机器人 3 之间的距离，并根据距离判断是否需要发布避免碰撞的速度指令。如果距离足够远，则计算并发布机器人 1 的速度，以使其沿预定轨迹移动。如果距离过近或机器人到达目标点，则调整机器人的速度或停止。函数还处理了笔的状态以及路径点的更新。最后，处理 ROS 2 回调并在循环结束后清理节点和关闭 ROS 2。

def main(args=None):
    rclpy.init(args=args)
    # 创建Controller类的实例
    con = Controller()
    # 主循环
    while (1):
        # 计算机器人3和机器人1之间的距离
        cx2 = con.pose3_x - con.pose1_x
        cy2 = con.pose3_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
                else:
                    bot1_index = 0
            elif distance < stop_threshold and bot1_index == 0:
                bot1_index += 1
                if (stop1_flag == 1 and stop2_flag == 1 and stop3_flag == 1):
                    con.reset_serv()
            last_err_x = err_x
            last_err_y = err_y
        rclpy.spin_once(con)
    con.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()