使用pykinect_azure与MoveIt运行Python脚本时出现段错误

运行结合pykinect_azure（追踪左手腕）和MoveIt（控制机械臂）的Python脚本时，启动阶段出现以下错误：

[ INFO] [1682280101.642647859]: Loading robot model 'panda'...
Segmentation fault (core dumped)

单独使用两个库时代码均可正常运行，但结合后程序快速崩溃，调试器无法捕获错误。

原代码 #

import sys
import cv2
import math
import rospy
import pykinect_azure as pykinect
import moveit_commander
from moveit_commander import MoveGroupCommander

pykinect.module_path = '/usr/lib/x86_64-linux-gnu/libk4a.so'

if __name__ == "__main__":
    # Initialize Kinect and MoveIt libraries
    pykinect.initialize_libraries(track_body=True)
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_robot')

    device_config = pykinect.default_configuration
    device_config.color_resolution = pykinect.K4A_COLOR_RESOLUTION_OFF
    device_config.depth_mode = pykinect.K4A_DEPTH_MODE_WFOV_2X2BINNED
    device = pykinect.start_device(config=device_config)
    bodyTracker = pykinect.start_body_tracker()
    arm = MoveGroupCommander('panda_arm')

    # Initialize variables
    cv2.namedWindow('Depth image with skeleton', cv2.WINDOW_NORMAL)
    left_wrist_init = None
    left_wrist_prev = None

    while True:
        # Update Kinect
        capture = device.update()
        body_frame = bodyTracker.update()
        ret_depth, depth_color_image = capture.get_colored_depth_image()
        ret_color, body_image_color = body_frame.get_segmentation_image()

        if not ret_depth or not ret_color:
            continue

        # Get left wrist position for each detected body
        num_bodies = body_frame.get_num_bodies()
        for body_idx in range(num_bodies):
            body = body_frame.get_body(body_idx)
            if body.is_valid:
                left_wrist_joint = body.joints[pykinect.K4ABT_JOINT_WRIST_LEFT]

                if left_wrist_init is None:
                    left_wrist_init = left_wrist_joint.position
                    left_wrist_prev = left_wrist_joint.position
                else:
                    #  Calculate the difference in wrist position
                    if left_wrist_joint.position is None:
                        continue
                    wrist_diff = [left_wrist_joint.position[i] - left_wrist_prev[i] for i in range(3)]

                    # Update the target pose of the robot arm
                    current_pose = arm.get_current_pose().pose
                    current_pose.position.x += wrist_diff[0]
                    current_pose.position.y += wrist_diff[1]
                    current_pose.position.z += wrist_diff[2]

                    # Move the robot arm to the new position
                    arm.set_pose_target(current_pose)
                    arm.go()

                    # Update the previous wrist position
                    left_wrist_prev = left_wrist_joint.position
    device.stop()
    moveit_commander.roscpp_shutdown()

可能的原因与解决方案 #

1. 库初始化顺序冲突 #

当前代码先初始化pykinect，再启动MoveIt/ROS，可能导致底层库（如K4A驱动、OpenCV）加载冲突。调换初始化顺序：先启动ROS和MoveIt，确保环境稳定后再初始化Kinect。

2. 线程资源竞争 #

MoveIt启动后会创建后台线程处理运动规划，Kinect的实时数据读取线程可能与之竞争共享资源（内存、GPU）。可以在MoveIt初始化后添加短暂延时，或给Kinect循环添加线程同步机制。

3. 未严格的空值检查 #

Kinect关节位置读取可能存在未初始化的指针，虽然判断了body.is_valid，但left_wrist_joint或其position仍可能为空，导致内存访问越界。需添加更严格的空值判断。

4. ROS节点初始化时机 #

rospy.init_node应放在moveit_commander.roscpp_initialize之后，确保ROS环境完全就绪后再加载机器人模型。

修复后的示例代码 #

import sys
import cv2
import math
import rospy
import pykinect_azure as pykinect
import moveit_commander
from moveit_commander import MoveGroupCommander
import time

if __name__ == "__main__":
    # 先初始化ROS和MoveIt
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_robot')
    
    # 加载机器人模型后延时1秒，确保后台线程稳定
    arm = MoveGroupCommander('panda_arm')
    time.sleep(1)

    # 再初始化Kinect
    pykinect.module_path = '/usr/lib/x86_64-linux-gnu/libk4a.so'
    pykinect.initialize_libraries(track_body=True)
    device_config = pykinect.default_configuration
    device_config.color_resolution = pykinect.K4A_COLOR_RESOLUTION_OFF
    device_config.depth_mode = pykinect.K4A_DEPTH_MODE_WFOV_2X2BINNED
    device = pykinect.start_device(config=device_config)
    bodyTracker = pykinect.start_body_tracker()

    # 初始化变量
    cv2.namedWindow('Depth image with skeleton', cv2.WINDOW_NORMAL)
    left_wrist_init = None
    left_wrist_prev = None

    while not rospy.is_shutdown():
        # 更新Kinect数据
        capture = device.update()
        body_frame = bodyTracker.update()
        ret_depth, depth_color_image = capture.get_colored_depth_image()
        ret_color, body_image_color = body_frame.get_segmentation_image()

        if not ret_depth or not ret_color:
            continue

        # 获取每个检测到的人体的左手腕位置
        num_bodies = body_frame.get_num_bodies()
        for body_idx in range(num_bodies):
            body = body_frame.get_body(body_idx)
            if not body.is_valid:
                continue
                
            left_wrist_joint = body.joints.get(pykinect.K4ABT_JOINT_WRIST_LEFT, None)
            if not left_wrist_joint or left_wrist_joint.position is None:
                continue

            if left_wrist_init is None:
                left_wrist_init = left_wrist_joint.position
                left_wrist_prev = left_wrist_joint.position
            else:
                # 计算手腕位置变化量
                wrist_diff = [left_wrist_joint.position[i] - left_wrist_prev[i] for i in range(3)]

                # 更新机械臂目标位姿
                current_pose = arm.get_current_pose().pose
                current_pose.position.x += wrist_diff[0]
                current_pose.position.y += wrist_diff[1]
                current_pose.position.z += wrist_diff[2]

                # 规划并执行运动，添加失败检查
                arm.set_pose_target(current_pose)
                success, plan = arm.plan()
                if success:
                    arm.execute(plan)

                # 更新上一帧手腕位置
                left_wrist_prev = left_wrist_joint.position
        
        # 处理窗口退出事件
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

    # 资源清理
    device.stop()
    cv2.destroyAllWindows()
    moveit_commander.roscpp_shutdown()

额外排查步骤 #

• 生成core dump：运行脚本前执行ulimit -c unlimited，崩溃后用gdb python core.xxx分析堆栈信息，定位错误位置。
• 单独验证组件：分别测试MoveIt模型加载、Kinect身体追踪功能，确认单独运行无问题。
• 检查权限：确保当前用户加入video组以访问Kinect设备，且拥有ROS节点操作权限。

内容的提问来源于stack exchange，提问作者InFumumVerti