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!