From 8f87992677c6a689176c28457b3d99fb46da1f49 Mon Sep 17 00:00:00 2001
From: gavanderhoorn <g.a.vanderhoorn@tudelft.nl>
Date: Sun, 21 Sep 2014 12:26:24 +0200
Subject: [PATCH] driver: remove Beautifulsoup dependency. Fix #36.
This change reimplements the load_joint_offsets(..) function using
lxml. A simple XPath expression is used to access the calibration_offset
elements.
Run-time behaviour should be identical. Tested with ur-sim.
Note: I opted to use lxml instead of urdfdom_py, as the latter has
some issues parsing the UR5/10 urdfs and we only need access to a single
custom element. Rosdep keys for python-lxml were recently added (for
urdfdom) for all (Ubuntu) versions, so this should not be an issue.
---
ur_driver/package.xml | 9 ++-------
ur_driver/src/ur_driver/driver.py | 26 ++++++++++++++++++--------
2 files changed, 20 insertions(+), 15 deletions(-)
diff --git a/ur_driver/package.xml b/ur_driver/package.xml
index dda1de3..ad68042 100644
--- a/ur_driver/package.xml
+++ b/ur_driver/package.xml
@@ -12,18 +12,13 @@
<author>Stuart Glaser</author>
<author email="sedwards@swri.org">Shaun Edwards</author>
-
<buildtool_depend>catkin</buildtool_depend>
-
+
<run_depend>rospy</run_depend>
<run_depend>actionlib</run_depend>
<run_depend>control_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>trajectory_msgs</run_depend>
<run_depend>ur_msgs</run_depend>
- <run_depend>python-beautifulsoup</run_depend>
-
-
- <export>
- </export>
+ <run_depend>python-lxml</run_depend>
</package>
diff --git a/ur_driver/src/ur_driver/driver.py b/ur_driver/src/ur_driver/driver.py
index 1ad08be..0072953 100755
--- a/ur_driver/src/ur_driver/driver.py
+++ b/ur_driver/src/ur_driver/driver.py
@@ -8,7 +8,6 @@ import struct
import traceback, code
import optparse
import SocketServer
-from BeautifulSoup import BeautifulSoup
import rospy
import actionlib
@@ -814,20 +813,28 @@ class URTrajectoryFollower(object):
# self.goal_handle.set_aborted(text="Took too long to reach the goal")
# self.goal_handle = None
+
# joint_names: list of joints
#
# returns: { "joint_name" : joint_offset }
def load_joint_offsets(joint_names):
+ from lxml import etree
robot_description = rospy.get_param("robot_description")
- soup = BeautifulSoup(robot_description)
-
+ doc = etree.fromstring(robot_description)
+
+ # select only 'calibration_offset' elements whose parent is a joint
+ # element with a specific value for the name attribute
+ expr = "/robot/joint[@name=$name]/calibration_offset"
result = {}
for joint in joint_names:
- try:
- joint_elt = soup.find('joint', attrs={'name': joint})
- calibration_offset = float(joint_elt.calibration_offset["value"])
+ joint_elt = doc.xpath(expr, name=joint)
+ if len(joint_elt) == 1:
+ calibration_offset = float(joint_elt[0].get("value"))
result[joint] = calibration_offset
- except Exception, ex:
+ rospy.loginfo("Found calibration offset for joint \"%s\": %.4f" % (joint, calibration_offset))
+ elif len(joint_elt) > 1:
+ rospy.logerr("Too many joints matched on \"%s\". Please report to package maintainer(s)." % joint)
+ else:
rospy.logwarn("No calibration offset for joint \"%s\"" % joint)
return result
@@ -888,7 +895,10 @@ def main():
# Reads the calibrated joint offsets from the URDF
global joint_offsets
joint_offsets = load_joint_offsets(joint_names)
- rospy.logerr("Loaded calibration offsets: %s" % joint_offsets)
+ if len(joint_offsets) > 0:
+ rospy.loginfo("Loaded calibration offsets from urdf: %s" % joint_offsets)
+ else:
+ rospy.loginfo("No calibration offsets loaded from urdf")
# Reads the maximum velocity
global max_velocity
--
GitLab