Segmentation fault (core dumped) error when running Python script with pykinect_azure and MoveIt

190 Views Asked by At

I'm running a Python script that uses pykinect_azure to track the position of the user's left wrist, and then tries to move a robot arm using the MoveIt library. However, when I run the script, I run into the following error:

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

Here is the code I'm using:

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()

Any ideas on what could cause this? When I use the libraries individually the code runs just fine, but now when combining the two the error occurs. I've tries to use the debugger to narrow it down, but it never catches the error but instead just shuts down shortly after it started. Thanks for any answer!

0

There are 0 best solutions below