第六章 实战项目

#!/usr/bin/env python
import rospy
import serial
import struct
import tf
from geometry_msgs.msg import Twist, Quaternion
from nav_msgs.msg import Odometry
import numpy as np

class BaseController:
    def __init__(self):
        rospy.init_node('base_controller')

        # 串口
        port = rospy.get_param('~port', '/dev/ttyACM0')
        baudrate = rospy.get_param('~baudrate', 115200)
        self.ser = serial.Serial(port, baudrate, timeout=0.1)

        # 机器人参数
        self.wheel_base = 0.3
        self.wheel_radius = 0.05

        # 里程计
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0
        self.last_time = rospy.Time.now()

        # 订阅/发布
        self.cmd_sub = rospy.Subscriber('cmd_vel', Twist, self.cmd_callback)
        self.odom_pub = rospy.Publisher('odom', Odometry, queue_size=10)
        self.tf_broadcaster = tf.TransformBroadcaster()

        rospy.loginfo("底盘控制节点启动")

    def cmd_callback(self, msg):
        """接收速度命令"""
        linear = msg.linear.x
        angular = msg.angular.z

        # 发送到Arduino
        data = struct.pack('ff', linear, angular)
        self.ser.write(data)

    def update_odometry(self):
        """更新里程计"""
        if self.ser.in_waiting >= 8:
            data = self.ser.read(8)
            vel_left, vel_right = struct.unpack('ff', data)

            # 正运动学
            v = (vel_left + vel_right) / 2.0
            omega = (vel_right - vel_left) / self.wheel_base

            # 时间间隔
            current_time = rospy.Time.now()
            dt = (current_time - self.last_time).to_sec()
            self.last_time = current_time

            # 位姿更新
            delta_x = v * np.cos(self.theta) * dt
            delta_y = v * np.sin(self.theta) * dt
            delta_theta = omega * dt

            self.x += delta_x
            self.y += delta_y
            self.theta += delta_theta

            # 发布里程计
            self.publish_odometry(v, omega, current_time)

    def publish_odometry(self, v, omega, current_time):
        """发布里程计消息"""
        # TF变换
        quat = tf.transformations.quaternion_from_euler(0, 0, self.theta)
        self.tf_broadcaster.sendTransform(
            (self.x, self.y, 0),
            quat,
            current_time,
            "base_link",
            "odom"
        )

        # Odometry消息
        odom = Odometry()
        odom.header.stamp = current_time
        odom.header.frame_id = "odom"
        odom.child_frame_id = "base_link"

        # 位置
        odom.pose.pose.position.x = self.x
        odom.pose.pose.position.y = self.y
        odom.pose.pose.orientation = Quaternion(*quat)

        # 速度
        odom.twist.twist.linear.x = v
        odom.twist.twist.angular.z = omega

        self.odom_pub.publish(odom)

    def run(self):
        rate = rospy.Rate(50)  # 50Hz

        while not rospy.is_shutdown():
            self.update_odometry()
            rate.sleep()

if __name__ == '__main__':
    try:
        controller = BaseController()
        controller.run()
    except rospy.ROSInterruptException:
        pass

#!/usr/bin/env python
import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from geometry_msgs.msg import PoseStamped

class ArUcoDetector:
    def __init__(self):
        rospy.init_node('aruco_detector')

        self.bridge = CvBridge()

        # 相机参数(标定得到)
        self.camera_matrix = np.array([
            [615.0, 0, 320.0],
            [0, 615.0, 240.0],
            [0, 0, 1.0]
        ])
        self.dist_coeffs = np.zeros(5)

        # ArUco字典
        self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
        self.parameters = cv2.aruco.DetectorParameters_create()

        # 订阅/发布
        self.image_sub = rospy.Subscriber('/camera/color/image_raw',
                                         Image, self.image_callback)
        self.pose_pub = rospy.Publisher('/target_pose',
                                       PoseStamped, queue_size=1)

    def image_callback(self, msg):
        # ROS图像转OpenCV
        cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')

        # 检测ArUco
        gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
        corners, ids, rejected = cv2.aruco.detectMarkers(
            gray, self.aruco_dict, parameters=self.parameters)

        if ids is not None:
            # 估计位姿
            rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
                corners, 0.05,  # 标记尺寸5cm
                self.camera_matrix,
                self.dist_coeffs
            )

            for i, marker_id in enumerate(ids):
                # 绘制
                cv2.aruco.drawAxis(cv_image, self.camera_matrix,
                                  self.dist_coeffs, rvecs[i], tvecs[i], 0.03)

                # 发布位姿
                pose = PoseStamped()
                pose.header.stamp = rospy.Time.now()
                pose.header.frame_id = "camera_link"

                pose.pose.position.x = tvecs[i][0][0]
                pose.pose.position.y = tvecs[i][0][1]
                pose.pose.position.z = tvecs[i][0][2]

                # 旋转向量转四元数
                rot_matrix, _ = cv2.Rodrigues(rvecs[i])
                quat = self.rotation_matrix_to_quaternion(rot_matrix)

                pose.pose.orientation.x = quat[0]
                pose.pose.orientation.y = quat[1]
                pose.pose.orientation.z = quat[2]
                pose.pose.orientation.w = quat[3]

                self.pose_pub.publish(pose)

                rospy.loginfo(f"检测到标记ID={marker_id[0]}, "
                            f"位置=({tvecs[i][0][0]:.2f}, "
                            f"{tvecs[i][0][1]:.2f}, "
                            f"{tvecs[i][0][2]:.2f})")

        # 显示
        cv2.imshow('ArUco Detection', cv_image)
        cv2.waitKey(1)

    def rotation_matrix_to_quaternion(self, R):
        """旋转矩阵转四元数"""
        trace = np.trace(R)

        if trace > 0:
            s = 0.5 / np.sqrt(trace + 1.0)
            w = 0.25 / s
            x = (R[2, 1] - R[1, 2]) * s
            y = (R[0, 2] - R[2, 0]) * s
            z = (R[1, 0] - R[0, 1]) * s
        else:
            if R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
                s = 2.0 * np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2])
                w = (R[2, 1] - R[1, 2]) / s
                x = 0.25 * s
                y = (R[0, 1] + R[1, 0]) / s
                z = (R[0, 2] + R[2, 0]) / s
            elif R[1, 1] > R[2, 2]:
                s = 2.0 * np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2])
                w = (R[0, 2] - R[2, 0]) / s
                x = (R[0, 1] + R[1, 0]) / s
                y = 0.25 * s
                z = (R[1, 2] + R[2, 1]) / s
            else:
                s = 2.0 * np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1])
                w = (R[1, 0] - R[0, 1]) / s
                x = (R[0, 2] + R[2, 0]) / s
                y = (R[1, 2] + R[2, 1]) / s
                z = 0.25 * s

        return [x, y, z, w]

    def run(self):
        rospy.spin()

if __name__ == '__main__':
    detector = ArUcoDetector()
    detector.run()

#!/usr/bin/env python
import rospy
import moveit_commander
from geometry_msgs.msg import PoseStamped

class PickAndPlace:
    def __init__(self):
        moveit_commander.roscpp_initialize([])
        rospy.init_node('pick_and_place')

        # MoveIt接口
        self.robot = moveit_commander.RobotCommander()
        self.scene = moveit_commander.PlanningSceneInterface()
        self.arm_group = moveit_commander.MoveGroupCommander("arm")
        self.gripper_group = moveit_commander.MoveGroupCommander("gripper")

        # 设置参数
        self.arm_group.set_max_velocity_scaling_factor(0.5)
        self.arm_group.set_max_acceleration_scaling_factor(0.5)

        # 订阅目标位姿
        self.target_sub = rospy.Subscriber('/target_pose',
                                          PoseStamped, self.target_callback)

        self.target_pose = None

    def target_callback(self, msg):
        self.target_pose = msg

    def open_gripper(self):
        """打开夹爪"""
        self.gripper_group.set_named_target("open")
        self.gripper_group.go(wait=True)
        rospy.sleep(0.5)

    def close_gripper(self):
        """关闭夹爪"""
        self.gripper_group.set_named_target("close")
        self.gripper_group.go(wait=True)
        rospy.sleep(0.5)

    def go_to_home(self):
        """回到初始位置"""
        self.arm_group.set_named_target("home")
        self.arm_group.go(wait=True)

    def pick(self, target_pose):
        """抓取流程"""
        rospy.loginfo("开始抓取...")

        # 1. 打开夹爪
        self.open_gripper()

        # 2. 移动到目标上方
        pre_grasp_pose = PoseStamped()
        pre_grasp_pose.header = target_pose.header
        pre_grasp_pose.pose = target_pose.pose
        pre_grasp_pose.pose.position.z += 0.1  # 上方10cm

        self.arm_group.set_pose_target(pre_grasp_pose)
        plan = self.arm_group.go(wait=True)

        if not plan:
            rospy.logwarn("无法规划到目标上方")
            return False

        rospy.sleep(1)

        # 3. 下降到目标
        self.arm_group.set_pose_target(target_pose)
        plan = self.arm_group.go(wait=True)

        if not plan:
            rospy.logwarn("无法下降到目标")
            return False

        rospy.sleep(1)

        # 4. 关闭夹爪(抓取)
        self.close_gripper()

        # 5. 提升
        self.arm_group.set_pose_target(pre_grasp_pose)
        self.arm_group.go(wait=True)

        rospy.loginfo("抓取完成")
        return True

    def place(self, place_pose):
        """放置流程"""
        rospy.loginfo("开始放置...")

        # 1. 移动到放置位置上方
        pre_place_pose = PoseStamped()
        pre_place_pose.header = place_pose.header
        pre_place_pose.pose = place_pose.pose
        pre_place_pose.pose.position.z += 0.1

        self.arm_group.set_pose_target(pre_place_pose)
        self.arm_group.go(wait=True)

        # 2. 下降
        self.arm_group.set_pose_target(place_pose)
        self.arm_group.go(wait=True)
        rospy.sleep(1)

        # 3. 打开夹爪(释放)
        self.open_gripper()

        # 4. 提升
        self.arm_group.set_pose_target(pre_place_pose)
        self.arm_group.go(wait=True)

        rospy.loginfo("放置完成")
        return True

    def run(self):
        rate = rospy.Rate(1)

        while not rospy.is_shutdown():
            if self.target_pose:
                # 执行抓取
                success = self.pick(self.target_pose)

                if success:
                    # 定义放置位置
                    place_pose = PoseStamped()
                    place_pose.header.frame_id = "base_link"
                    place_pose.pose.position.x = 0.3
                    place_pose.pose.position.y = 0.2
                    place_pose.pose.position.z = 0.1
                    place_pose.pose.orientation.w = 1.0

                    # 执行放置
                    self.place(place_pose)

                    # 回到初始位置
                    self.go_to_home()

                self.target_pose = None

            rate.sleep()

if __name__ == '__main__':
    try:
        pnp = PickAndPlace()
        pnp.run()
    except rospy.ROSInterruptException:
        pass

# 限制加速度
def limit_acceleration(current, target, max_accel, dt):
    max_change = max_accel * dt
    change = target - current

    if abs(change) > max_change:
        return current + np.sign(change) * max_change
    return target

# 低通滤波
alpha = 0.2
filtered = alpha * new_value + (1 - alpha) * old_value

class SafetyMonitor:
    def check_safety(self):
        # 检查倾斜
        if abs(imu.roll) > 30:
            emergency_stop()

        # 检查卡死
        if motor_current > threshold:
            backward_escape()

        # 检查通信超时
        if time.time() - last_cmd > 1.0:
            stop_motors()