Issue with PyKDL

[julianraheema]

I have this error after launching: roslaunch spot_driver driver.launch

My settings:
ROS: Melodic
Python: 2, 3.6, and 3.8 are installed
spot sdk: 2.3.5

I tried the solution in here orocos/orocos_kinematics_dynamics#115, but no luck.

Traceback (most recent call last):
File "/home/user1/src/catkin_ws/src/spot_ros/spot_driver/scripts/spot_ros", line 3, in
from spot_driver.spot_ros import SpotROS
File "/home/user1/src/catkin_ws/src/spot_ros/spot_driver/src/spot_driver/spot_ros.py", line 24, in
import tf2_geometry_msgs
File "/home/user1/src/catkin_ws/devel/lib/python3/dist-packages/tf2_geometry_msgs/init.py", line 34, in
exec(__fh.read())
File "", line 1, in
File "/home/user1/src/catkin_ws/src/geometry2/tf2_geometry_msgs/src/tf2_geometry_msgs/tf2_geometry_msgs.py", line 31, in
import PyKDL
ImportError: dynamic module does not define module export function (PyInit_PyKDL)
[spot/spot_ros-6] process has died [pid 14553, exit code 1, cmd /home/user1/src/catkin_ws/src/spot_ros/spot_driver/scripts/spot_ros joint_states:=/joint_states tf:=/tf __name:=spot_ros __log:=/home/user1/.ros/log/7a8bb8da-a67b-11ed-bc52-9cb6d06c21cf/spot-spot_ros-6.log].
log file: /home/user1/.ros/log/7a8bb8da-a67b-11ed-bc52-9cb6d06c21cf/spot-spot_ros-6*.log

Thank you.

[heuristicus]

It may be that some of the points mentioned at #99 can help with this. But your best option is to switch to using noetic if you can. Also, you should update your SDK. 2.3.5 is now close to 2 years old.

[dcolli23]

Hi, Is this driver tested with the newest Spot SDK? We're attempting to use this driver in our lab with Spot and Spot Core running version 3.2.X and are running into this PyKDL issue. This is further complicated by the fact that the Core runs Ubuntu 18.04 so we're somewhat forced to use Melodic over Noetic unless we want to jump into using Docker specifically for this driver.

I ask as we're weighing our options on if we should simply use the provided Python API and abandon ROS integration thanks to using a newer Spot and Core.

I'm asking here as I imagine anyone else using a Spot Core with 18.04 would run into similar issues.

Thanks!

[heuristicus]

Most of the driver is tested with the latest SDK. I don't have access to a spot core so I'm not sure what differences exist there. You might try using the melodic build instructions at https://github.com/heuristicus/spot_ros#building-for-melodic, but I've never personally used the driver on melodic so can't confirm if that actually works.

Part of the problem here is that the BD SDK expects use of python3, while the default on melodic is 2.7, which can lead to awkward mismatches.

There are notes here about installing the package pyKDL package from source, which may be an alternative option.

As a note, the place where we are currently using tf2_geometry_msgs is to transform poses to the body frame:

spot_ros/spot_driver/src/spot_driver/spot_ros.py

Line 594 in bb82c29

pose_in_body = tf2_geometry_msgs.do_transform_pose(pose, body_to_fixed)

These functions are only used to send trajectory commands to the robot to move if from where it currently is to a point relative to its current position, so if you do not need those, you can remove these lines and avoid the import:

spot_ros/spot_driver/src/spot_driver/spot_ros.py

Lines 751 to 888 in bb82c29

	def trajectory_callback(self, msg):
	"""
	Handle a callback from the trajectory topic requesting to go to a location
	The trajectory will time out after 5 seconds
	Args:
	msg: PoseStamped containing desired pose
	Returns:
	"""
	if not self.robot_allowed_to_move():
	rospy.logerr(
	"Trajectory topic received a message but the robot is not allowed to move."
	)
	return
	try:
	self._send_trajectory_command(
	self._transform_pose_to_body_frame(msg), rospy.Duration(5)
	)
	except tf2_ros.LookupException as e:
	rospy.logerr(str(e))
	def handle_trajectory(self, req):
	"""ROS actionserver execution handler to handle receiving a request to move to a location"""
	if not self.robot_allowed_to_move():
	rospy.logerr(
	"Trajectory service was called but robot is not allowed to move"
	)
	self.trajectory_server.set_aborted(
	TrajectoryResult(False, "Robot is not allowed to move.")
	)
	return
	target_pose = req.target_pose
	if req.target_pose.header.frame_id != "body":
	rospy.logwarn("Pose given was not in the body frame, will transform")
	try:
	target_pose = self._transform_pose_to_body_frame(target_pose)
	except tf2_ros.LookupException as e:
	self.trajectory_server.set_aborted(
	TrajectoryResult(False, "Could not transform pose into body frame")
	)
	return
	if req.duration.data.to_sec() <= 0:
	self.trajectory_server.set_aborted(
	TrajectoryResult(False, "duration must be larger than 0")
	)
	return
	cmd_duration = rospy.Duration(req.duration.data.secs, req.duration.data.nsecs)
	resp = self._send_trajectory_command(
	target_pose, cmd_duration, req.precise_positioning
	)
	def timeout_cb(trajectory_server, _):
	trajectory_server.publish_feedback(
	TrajectoryFeedback("Failed to reach goal, timed out")
	)
	trajectory_server.set_aborted(
	TrajectoryResult(False, "Failed to reach goal, timed out")
	)
	# Abort the actionserver if cmd_duration is exceeded - the driver stops but does not provide feedback to
	# indicate this so we monitor it ourselves
	cmd_timeout = rospy.Timer(
	cmd_duration,
	functools.partial(timeout_cb, self.trajectory_server),
	oneshot=True,
	)
	# Sleep to allow some feedback to come through from the trajectory command
	rospy.sleep(0.25)
	if self.spot_wrapper._trajectory_status_unknown:
	rospy.logerr(
	"Sent trajectory request to spot but received unknown feedback. Resending command. This will "
	"only be attempted once"
	)
	# If we receive an unknown result from the trajectory, something went wrong internally (not
	# catastrophically). We need to resend the command, because getting status unknown happens right when
	# the command is sent. It's unclear right now why this happens
	resp = self._send_trajectory_command(
	target_pose, cmd_duration, req.precise_positioning
	)
	cmd_timeout.shutdown()
	cmd_timeout = rospy.Timer(
	cmd_duration,
	functools.partial(timeout_cb, self.trajectory_server),
	oneshot=True,
	)
	# The trajectory command is non-blocking but we need to keep this function up in order to interrupt if a
	# preempt is requested and to return success if/when the robot reaches the goal. Also check the is_active to
	# monitor whether the timeout_cb has already aborted the command
	rate = rospy.Rate(10)
	while (
	not rospy.is_shutdown()
	and not self.trajectory_server.is_preempt_requested()
	and not self.spot_wrapper.at_goal
	and self.trajectory_server.is_active()
	and not self.spot_wrapper._trajectory_status_unknown
	):
	if self.spot_wrapper.near_goal:
	if self.spot_wrapper._last_trajectory_command_precise:
	self.trajectory_server.publish_feedback(
	TrajectoryFeedback("Near goal, performing final adjustments")
	)
	else:
	self.trajectory_server.publish_feedback(
	TrajectoryFeedback("Near goal")
	)
	else:
	self.trajectory_server.publish_feedback(
	TrajectoryFeedback("Moving to goal")
	)
	rate.sleep()
	# If still active after exiting the loop, the command did not time out
	if self.trajectory_server.is_active():
	cmd_timeout.shutdown()
	if self.trajectory_server.is_preempt_requested():
	self.trajectory_server.publish_feedback(TrajectoryFeedback("Preempted"))
	self.trajectory_server.set_preempted()
	self.spot_wrapper.stop()
	if self.spot_wrapper.at_goal:
	self.trajectory_server.publish_feedback(
	TrajectoryFeedback("Reached goal")
	)
	self.trajectory_server.set_succeeded(TrajectoryResult(resp[0], resp[1]))
	else:
	self.trajectory_server.publish_feedback(
	TrajectoryFeedback("Failed to reach goal")
	)
	self.trajectory_server.set_aborted(
	TrajectoryResult(False, "Failed to reach goal")
	)

If you do need the trajectory commands, there are a few options:

The code in the geometry msgs is here.

def do_transform_pose(pose, transform):
    f = transform_to_kdl(transform) * PyKDL.Frame(PyKDL.Rotation.Quaternion(pose.pose.orientation.x, pose.pose.orientation.y,
                                                                          pose.pose.orientation.z, pose.pose.orientation.w),
                                                PyKDL.Vector(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z))
    res = PoseStamped()
    res.pose.position.x = f[(0, 3)]
    res.pose.position.y = f[(1, 3)]
    res.pose.position.z = f[(2, 3)]
    (res.pose.orientation.x, res.pose.orientation.y, res.pose.orientation.z, res.pose.orientation.w) = f.M.GetQuaternion()
    res.header = transform.header
    return res

In theory this could be replaced with a custom function to multiply transforms which does not use KDL.

Because we're using this function to transform frames which are not in the body frame to the body frame, another option is to remove the use of the _transform_pose_to_body_frame function entirely within the driver, and simply reject any transforms received which are not in the body frame.

[julianraheema]

Switching to Noetic solved my problem. Thank you.

[dcolli23]

Thanks for the in-depth answer, @heuristicus! We'll try building with those instructions and if that doesn't work, we'll try using a Docker container that utilizes Noetic.