From 27cd1d9c65eee2667515e2da940dc391cfd9ac4b Mon Sep 17 00:00:00 2001
From: Michael Lautman <mike@picknik.ai>
Date: Thu, 14 Mar 2019 15:03:29 -0600
Subject: [PATCH 01/11] trim whitespace
---
CMakeLists.txt | 4 +--
package.xml | 2 +-
src/srdfdom/srdf.py | 34 ++++++++++----------
test/__pycache__/test.cpython-36-PYTEST.pyc | Bin 0 -> 8285 bytes
test/resources/pr2_desc.urdf | 8 ++---
test/test.py | 32 +++++++++---------
6 files changed, 40 insertions(+), 40 deletions(-)
create mode 100644 test/__pycache__/test.cpython-36-PYTEST.pyc
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6f61272..620e34c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -29,7 +29,7 @@ catkin_package(
DEPENDS TinyXML console_bridge urdfdom_headers
)
-add_library(${PROJECT_NAME}
+add_library(${PROJECT_NAME}
src/model.cpp
src/srdf_writer.cpp
)
@@ -44,7 +44,7 @@ install(DIRECTORY include/${PROJECT_NAME}/
FILES_MATCHING PATTERN "*.h"
)
-install(PROGRAMS
+install(PROGRAMS
scripts/display_srdf
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
diff --git a/package.xml b/package.xml
index 39494df..1d57277 100644
--- a/package.xml
+++ b/package.xml
@@ -10,7 +10,7 @@
<url type="website">http://ros.org/wiki/srdfdom</url>
<url type="bugtracker">https://github.com/ros-planning/srdfdom/issues</url>
<url type="repository">https://github.com/ros-planning/srdfdom</url>
-
+
<buildtool_depend>catkin</buildtool_depend>
<build_depend>boost</build_depend>
diff --git a/src/srdfdom/srdf.py b/src/srdfdom/srdf.py
index 7e9f5c1..4e97c40 100644
--- a/src/srdfdom/srdf.py
+++ b/src/srdfdom/srdf.py
@@ -49,16 +49,16 @@ def __init__(self, center = None, radius = 0.0):
class VirtualJoint(xmlr.Object):
TYPES = ['unknown', 'fixed', 'floating', 'planar']
-
+
def __init__(self, name = None, child_link = None, parent_frame = None, joint_type = None):
self.name = name
self.child_link = child_link
self.parent_frame = parent_frame
self.type = joint_type
-
+
def check_valid(self):
assert self.type in self.TYPES, "Invalid joint type: {}".format(self.type)
-
+
# Aliases
@property
def joint_type(self): return self.type
@@ -94,7 +94,7 @@ def __init__(self, name = None, group = None, parent_link = None, parent_group =
name_attribute,
xmlr.Attribute('group', str),
xmlr.Attribute('parent_link', str),
- xmlr.Attribute('parent_group', str, False)
+ xmlr.Attribute('parent_group', str, False)
])
class PassiveJoint(xmlr.Object):
@@ -148,8 +148,8 @@ def __init__(self, name = None, group = None):
xmlr.AggregateElement('joint', JointVal),
xmlr.Attribute('group', str)
])
-
-
+
+
class LinkSphereApproximation(xmlr.Object):
def __init__(self, link = None):
self.aggregate_init()
@@ -160,11 +160,11 @@ def __init__(self, link = None):
xmlr.Attribute('link', str),
xmlr.AggregateElement('sphere', Sphere)
])
-
+
class Robot(xmlr.Object):
def __init__(self, name = None):
self.aggregate_init()
-
+
self.name = name
self.groups = []
self.group_states = []
@@ -175,10 +175,10 @@ def __init__(self, name = None):
self.link_sphere_approximations = []
self.group_map = {}
self.group_state_map = {}
-
+
def add_aggregate(self, typeName, elem):
xmlr.Object.add_aggregate(self, typeName, elem)
-
+
if typeName == 'group':
group = elem
self.group_map[group.name] = group
@@ -191,22 +191,22 @@ def add_link(self, link):
def add_joint(self, joint):
self.add_aggregate('joint', joint)
-
+
def add_chain(self, chain):
self.add_aggregate('chain', chain)
-
+
def add_group(self, group):
self.add_aggregate('group', group)
-
+
def add_passive_joint(self, joint):
self.add_aggregate('passive_joint', joint)
-
+
def add_disable_collisions(self, col):
self.add_aggregate('disable_collisions', col)
-
+
def add_link_sphere_approximation(self, link):
self.add_aggregate('link_sphere_approximation', link)
-
+
@classmethod
def from_parameter_server(cls, key = 'robot_description_semantic'):
@@ -219,7 +219,7 @@ def from_parameter_server(cls, key = 'robot_description_semantic'):
# Could move this into xml_reflection
import rospy
return cls.from_xml_string(rospy.get_param(key))
-
+
xmlr.reflect(Robot, tag = 'robot', params = [
# name_attribute,
xmlr.Attribute('name', str, False), # Is 'name' a required attribute?
diff --git a/test/__pycache__/test.cpython-36-PYTEST.pyc b/test/__pycache__/test.cpython-36-PYTEST.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..71aea4fc872807796493ae5805515a25829a39bc
GIT binary patch
literal 8285
zcmd5>&2JmW72jPh$t6YV+p;7(4nrhC)D}`~2W^v5j$6x4n-3+8osVuEY*w75L~6;U
zXP34lkmx}MQWQO;haS>X(L;dd&`W_{bM8M-pwmNv_7VX-<XHI9{@(18+$AX|ZiAvE
z=I!pAuQ%^&-n_Y7EapG`^k=vCo=}v(E3u!B_p2!WCjd-gs;ii)d}@tULpRe*Gc)o#
zYi8xsFb(<4nK?XDZoWBXPN~#acMHv;S(NXpJ8jMYmv(2HbLJdy8J4}TnDfkFIXo9w
zo=xGo$O^28=NUH5X7F5kNnx{W?!Ln2oLuAVamrkFR@nT$#y`gQ>al{6)SJrM!U^bA
zwnf(Gtj4_Nnkw|0Z*JYdbE<7~-?<s^dTSS-g<U7GxZ`^rUUjM}b;D1K$~8Rwb%3tY
zP<rY?xvTPLx@toW<oX+F%v|bXM&L3%^^WllR*L!6?X|4P*XrC4uI<%bCXANHoLh%&
zN2Cs#u1J9<p>G2x3fEt~{@V5JTh{jLTi3-@%Q*;c)^}W@P7^7|4TM_#t-@zO#Dn#X
zJ+J9(H0%4$#y#I^dUu_AaB0KmRTR0<jll5(3D(<(VnI@1Rfz=4ah;~q3g&1@1pr0O
z;6JPCYF_1p{jXLJ?!;H0;WP!$5-<ql2;>P&0jz1fNbl1GXlWss#hX|p7Fd4BgFe+O
zs2Qhv9@r|$ihgMnnly#t*C5G2`Ki)Sk5xP&v14^V#Y=(4@CGf>ZYm!rHI=29e%}aE
z{kF&ImU682v=3C&r3ZDge^i(1Y7Lz<tqTjF=fOXze^)!k$Xm*LpY`;v*40;_(<^Ax
zf5Ys5!0g#cbv11XvxZb1z8R#0Ow`I)0UlrBDDrC=7?#`DKU8@J<K>4fhppn{y`$W@
zt1CfvKaU=_RnQB{L;5h@u+M%9_3wU}5r2dc-$jA0PeIoU+ea6+>b07~p<Sg`r@7<s
zl2<F)0c>oi1BJYi&@7VE0?FtZfL&PDd!?QVNkLX!aIm4p-cvR07up^DW~d))y2uYl
z6?#2zU_~vv>4@}Q+wC~87;XJoq1Iv8sy)=H2l<Ly_k*g}Y+rebrY4=qC`ZrOu4@fP
zw3>EM-CM7?Ue$K}D|qR5`pOo-Y2|#8z!?Hd05FEL^uA2*Abe*?I9~<sX_Sa!P7dnm
z$sxN3#FvN-{tKn2{F2>bc$`qv-CzFc>CdLt=dK(*HK~teTD;S(HAAFarzOc0={B5X
zAky4!?K(pDJsyaZz2l2?&Gqa+<liQyU+3K8YiUj<%Fh9q8Jn2uo0%Q?lnaw<!b%6N
zcxddCB=7_Vk=miCi>d(!bsm2M|9N#;E#X^JzlNs)=e3IGw921EkAWi~;nKz>o%hFh
z>B2AY*{({Ewg>w5&&%%;IRbL-aNr6|bSr>M=zqt}2sjOGG_=`68@>M>Zxf&=R9N%x
z(x`zRq`PFkX;N!oe;#2o5}S#zzagyLw9?zRZX7*6loM?kr2u;dNr;7FWk=;<gUF+g
ziGD2`I^GLdd${v#Lpdaq#B|gNy9X(|bo`wBXhJqgm&{26zlGwL0Ti_W5rkz?z7fo$
z%tuAGpDd2wp+9`35`ZCmHPDD<uz8)>29|++vgWx!Lz@(_2tGCx5xim}+b|kA?744H
zeZLpP60dWq4mjB_$VVdiB$x_4Kl&AdVq=>8ZGgxGc#WkNK?VGxI;>;xIxG$Umf1ck
zzDB$%)$F?KuooVvn<v{?$BkQSGa_I2>n%U9TUBiBe!#^P_V<vtj?YOA#SHVR9ol-Y
zhdc13T~QRbUcdI%AP$fm<e~iTarikIH28<U$oK*CI>E!F^>s}8GKyaUP#~FES!N+K
zDtQ=AC_RFlT34xI|3~I?6~+GsfFt8QQ)4RA5PzlQeyNx`_Qnjp(=5vjJToi@PUXe)
zErcHwXuUx}79Y4x;={lsX<VlEFk(*&BCp>gqa$s=p2||kDt1%sr`QoGn$oxJ7JBDP
z(XYb29S>U#lzOw=Jd|JME5qtt?seKx^^V65V|ArcrQyM;MYS(qESENhJx6OVJ+$`n
zXl-@RuD41%w(nSo-}X1lk}``B<x)^@C$RV)?H{Mx=FLQPWkc2uX|2d_3#u15(Q1wy
z?v+atzgZqx_}H?g@82vxo17)V94!A!v;rF@w+bl^DU8wWv=ECTT&M;fj|eB;IN%XN
zbC^lQCdr^^F_IfmZG3%k;Z-IK>;-!cpE!EIX27p8hTpC8pkupMoFxeyWD4iJS(ck2
z;`IUjH7+%iBw)$+aw!bKL#-H_iPe33$91f#=OW^$do902EiZ1C<)bXWE`?u7rK?Qn
z${pMHTAStXvW8s+JIkfhM@uR=;+@vM<+s5z$FkdP?j6*zOMoCb!hoEZ#eAtQm}Mw`
zvf)ZNM~TRD_-6Uy`m?1=D3{lvj@)MTjxUi3-F#Tb=i(hPQlevJ<J5Hx$*OEfV;v%F
zfrCW;R96$4-?)<*v$Kd<N&JxSN&HZ{l}C0a{fVS~M1&1hE#^NS!E+?3gR>m7^kj=k
z9wXUe+*?GH4gEyYkmDX8^!Ow8obU%@9z=2_Ibt|U-vNv}o_@^)*HB3~fN`r1hfXwl
zm{%C}AE7&&WYk~i4<4x##5C?DLQb7(Did75qZ$7r8~te=PBHd}n)#S{-@u85G6iC$
z#=WLRK^vkscySyteS|K-E#7ftaAXz(FIJC3sO~#%&CF5C#bS0~3&S~RV?T2k&Po)T
zga<eoM9WjoG-YM1S_h|1>hwP5^(iZ&tm>;2Iv+ijjGNYz?FV6023@GglMSWh0t8YN
zFyGdCNLqwRsjkK}I>j*kINeL(^pRreW38uC_}tYS#xX+OUK(#XnZ!^U`CzJ>ZWNGK
z$n49Y3!zwuD^9>6a2{(nGXXa{#K|BoyDf~iUEQ~L9ULJ2J#6(N*H66pM>B1H$zqOQ
zUB9?aJQ6CGhuyU~P6-2%dgGOsgy#7|Z`;A1&}ml|y4S{8mBh#|Q6n8%4H;Bj4-&jg
z=*I{>oiIqm(9&i2$o)~E7&;zbER64^z9`0n3V$ubpQ8a)OBABDSdyG{jVzynb5PLa
z-=wLNXDBf%W$M?PZS3^4!jI6%r}Mvp6Kn=0k7D4&8~%?TpQID9{{MGcksZ+<(uz?W
zX+?yST48MSZxgegC-4G5Oc5?gMd06|s`CUWqY>!?uMlFBz!d^761YlWpbh-H^!Ysk
z16AN;Ied%2bpkgCyaW(QmVciRKOjISDE<n8)1}#T5UMn#G(SemN0w%+|3I2~3^cIu
z+gQ#2z{W?VL^f_rkY)@Ak1@k?D@tRQ<wsi1C3}-^kuB%h)Uf3MyTA$|_TV4D6_G&w
zTQAeiH0D`xU;9w?3-W!M-jQk#JyWK!h&lS+irUV|`m2NbGcg*depc4MKd4_C=|3mu
zI*@bWIstS2deHW4tZjdO>c0@JhQ?n+QsVPLkND{Q9x;mUe2&y<u9wAnrzK~WK|iHm
zKW7<W(SvHbS#XzX20nR!oo8o~SeEH#mlfF_E0w8?Xp}+CJX;EDf)#d_E#FrLqb*U&
zn<>~MjA#W$a>ZR-${?M+GU&hB?>`uQI7TiSBZo20#c0XNG0rD2NRT6yJ)}3skdkH>
zB22n57wHYnj;odrQ+%G~T3U=tbCOH*bOjV!b%<S_h+Somg>9w(dHf;%=kkdEIicH(
z6H1RrPb3UGI$?>admhIPi##csnYL@UY;GE^QwuCa;AW23clW|q9pBi)C?#M{yH<R6
zF{gRr`-HAc%>2DQ4;u!ZCCyyWsqWK=7=bQ=20nvmBERQTp{x;->PtPO(;G;jKr<<G
zslJWG{~aW9kWkUgMMNTdUdLq)#}3vCN#90RX*4S>(sETJyV-+WW!J$yO`Bi&3(ZLc
z=*rRMNm~|W-CgHEddXOBz2ATLS0B;uk1zh`tKmPy^pl9^Z=qs@bpAGQe?^IiJ|V_M
zMEv<hbB<d`xFK5OIE>3o#L2(#TRSJikgsst5t%Rqm)nnIyU^)8U}ogW&qsXO*$D#`
zGb3f-n`w&95j{yXZa#2PX3~9TKWVa-m+{HzyV#GhR*|l^m~$Z1yF$G;!W=Otqm;PF
zeow5r4KO)OSyC6ZC3Q(FAT_^;ykc1BeGvi8qPncfyfC#vvVK)Nk1X-LRw5|hE&#uz
z9+kdGbLc|Vuq+C*EKB4qTz;|+3<HsvW!*uZCamF<rsl5_c#XjO1cs5=ASCA(QSly%
zjKhtT@w`zqiV{oHG?B=&thIB(*y+^W02y9UxY|Cn245>y`;b_*9v&8uB3b8-%+!k9
z041`0TppbzXKfx+p$*J0j83Z_P@P$zs1O&!q1z#~6f-!NtP`qPZ`JV(6Ux~LD{ofK
z^I!>*W>Tm`)t;m!ywLVTt)<&tndFyu0AX^O^1i|#wqJuYk}Kyn;dEO3B%TSQ5;FaY
k^fgbSv4NWuU8AcR$|)CALo1{W`Yjm?#=`thlo><)H^D~}Z~y=R
literal 0
HcmV?d00001
diff --git a/test/resources/pr2_desc.urdf b/test/resources/pr2_desc.urdf
index cbc5c77..abd561b 100644
--- a/test/resources/pr2_desc.urdf
+++ b/test/resources/pr2_desc.urdf
@@ -73,10 +73,10 @@
<!-- Now we can start using the macros included above to define the actual PR2 -->
<!-- The first use of a macro. This one was defined in base.urdf.xacro above.
A macro like this will expand to a set of link and joint definitions, and to additional
- Gazebo-related extensions (sensor plugins, etc). The macro takes an argument, name,
- that equals "base", and uses it to generate names for its component links and joints
- (e.g., base_link). The included origin block is also an argument to the macro. By convention,
- the origin block defines where the component is w.r.t its parent (in this case the parent
+ Gazebo-related extensions (sensor plugins, etc). The macro takes an argument, name,
+ that equals "base", and uses it to generate names for its component links and joints
+ (e.g., base_link). The included origin block is also an argument to the macro. By convention,
+ the origin block defines where the component is w.r.t its parent (in this case the parent
is the world frame). For more, see http://www.ros.org/wiki/xacro -->
<link name="base_link">
<inertial>
diff --git a/test/test.py b/test/test.py
index 4900bbe..7a38873 100755
--- a/test/test.py
+++ b/test/test.py
@@ -8,7 +8,7 @@
from xml.dom.minidom import parseString
import xml.dom
-# xml match code from test_xacro.py
+# xml match code from test_xacro.py
# by Stuart Glaser and William Woodall
def first_child_element(elt):
@@ -18,7 +18,7 @@ def first_child_element(elt):
return c
c = c.nextSibling
return None
-
+
def next_sibling_element(elt):
c = elt.nextSibling
while c:
@@ -85,7 +85,7 @@ def xml_matches(a, b):
b.writexml(sys.stdout)
return False
return True
-
+
## A python unit test for srdf
class TestSRDFParser(unittest.TestCase):
## test valid srdf
@@ -145,7 +145,7 @@ def test_full_srdf(self):
'''
robot = SRDF.from_xml_string(srdf_data)
self.assertTrue( xml_matches(robot.to_xml_string(),expected))
-
+
def test_simple_srdf(self):
datadir=rospkg.RosPack().get_path('srdfdom')+"/test/resources/"
stream = open(datadir+'pr2_desc.1.srdf', 'r')
@@ -156,7 +156,7 @@ def test_simple_srdf(self):
self.assertTrue(len(robot.group_states)==0)
self.assertTrue(len(robot.disable_collisionss)==0)
self.assertTrue(len(robot.end_effectors)==0)
-
+
stream = open(datadir+'pr2_desc.2.srdf', 'r')
robot = SRDF.from_xml_string(stream.read())
stream.close()
@@ -165,7 +165,7 @@ def test_simple_srdf(self):
self.assertTrue(len(robot.group_states)==0)
self.assertTrue(len(robot.disable_collisionss)==0)
self.assertTrue(len(robot.end_effectors)==0)
-
+
def test_complex_srdf(self):
datadir=rospkg.RosPack().get_path('srdfdom')+"/test/resources/"
stream = open(datadir+'pr2_desc.3.srdf', 'r')
@@ -177,10 +177,10 @@ def test_complex_srdf(self):
self.assertTrue(len(robot.disable_collisionss)==2)
self.assertTrue(robot.disable_collisionss[0].reason=="adjacent")
self.assertTrue(len(robot.end_effectors)==2)
-
+
self.assertTrue(robot.virtual_joints[0].name=="world_joint")
self.assertTrue(robot.virtual_joints[0].type=="planar")
-
+
for group in robot.groups:
if (group.name == "left_arm" or group.name == "right_arm" ):
self.assertTrue(len(group.chains)==1)
@@ -194,31 +194,31 @@ def test_complex_srdf(self):
if group.name == "whole_body" :
self.assertTrue(len(group.joints)==1)
self.assertTrue(len(group.subgroups)==2)
-
+
index=0
if robot.group_states[0].group !="arms":
index=1
-
+
self.assertTrue(robot.group_states[index].group =="arms")
self.assertTrue(robot.group_states[index].name =="tuck_arms")
self.assertTrue(robot.group_states[1-index].group =="base")
self.assertTrue(robot.group_states[1-index].name =="home")
-
- v=next((joint.value for joint in robot.group_states[index].joints if joint.name=="l_shoulder_pan_joint"),None)
+
+ v=next((joint.value for joint in robot.group_states[index].joints if joint.name=="l_shoulder_pan_joint"),None)
self.assertTrue(len(v) == 1)
self.assertTrue(v[0] ==0.2)
-
- w=next((joint.value for joint in robot.group_states[1-index].joints if joint.name=="world_joint"),None)
+
+ w=next((joint.value for joint in robot.group_states[1-index].joints if joint.name=="world_joint"),None)
self.assertTrue(len(w) == 3)
self.assertTrue(w[0] ==0.4)
self.assertTrue(w[1] ==0)
self.assertTrue(w[2] ==-1)
-
+
index = 0 if (robot.end_effectors[0].name[0] == 'r') else 1
self.assertTrue(robot.end_effectors[index].name == 'r_end_effector')
self.assertTrue(robot.end_effectors[index].group == 'r_end_effector')
self.assertTrue(robot.end_effectors[index].parent_link == 'r_wrist_roll_link')
-
+
if __name__ == '__main__':
import rostest
From 9cf1b2462bb56d8e83e9f312fafa8b8d6caa1ebe Mon Sep 17 00:00:00 2001
From: Michael Lautman <mike@picknik.ai>
Date: Thu, 14 Mar 2019 15:04:27 -0600
Subject: [PATCH 02/11] fixing whitespace in python file
---
test/test.py | 198 +++++++++++++++++++++++++--------------------------
1 file changed, 99 insertions(+), 99 deletions(-)
diff --git a/test/test.py b/test/test.py
index 7a38873..2947335 100755
--- a/test/test.py
+++ b/test/test.py
@@ -91,33 +91,33 @@ class TestSRDFParser(unittest.TestCase):
## test valid srdf
def test_full_srdf(self):
- srdf_data = '''
- <robot name="myrobot">
- <group name="body">
- <joint name="J1" />
- <joint name="J2" />
- <joint name="J3" />
- <chain base_link="robot_base" tip_link="robot_tip" />
- <group name="arm" />
- </group>
- <group_state name="zero" group="body">
- <joint name="J1" value="0" />
- <joint name="J2" value="0" />
- <joint name="J3" value="0" />
- </group_state>
- <end_effector name="tip_ee" parent_link="tip" group="arm" parent_group="body" />
- <end_effector name="othertip_ee" parent_link="othertip" group="arm" />
- <virtual_joint name="virtual_joint" type="floating" parent_frame="body_frame" child_link="arm" />
- <disable_collisions link1="link1" link2="link3" />
- <disable_collisions reason="Adjacent" link1="link1" link2="link2" />
- <link_sphere_approximation link="link1" />
- <link_sphere_approximation link="link2" >
- <sphere center="1.0 2.0 3.0" radius="1.0" />
- <sphere center="1.0 2.0 4.0" radius="2.0" />
- </link_sphere_approximation>
- </robot>
- '''
- expected = '''
+ srdf_data = '''
+ <robot name="myrobot">
+ <group name="body">
+ <joint name="J1" />
+ <joint name="J2" />
+ <joint name="J3" />
+ <chain base_link="robot_base" tip_link="robot_tip" />
+ <group name="arm" />
+ </group>
+ <group_state name="zero" group="body">
+ <joint name="J1" value="0" />
+ <joint name="J2" value="0" />
+ <joint name="J3" value="0" />
+ </group_state>
+ <end_effector name="tip_ee" parent_link="tip" group="arm" parent_group="body" />
+ <end_effector name="othertip_ee" parent_link="othertip" group="arm" />
+ <virtual_joint name="virtual_joint" type="floating" parent_frame="body_frame" child_link="arm" />
+ <disable_collisions link1="link1" link2="link3" />
+ <disable_collisions reason="Adjacent" link1="link1" link2="link2" />
+ <link_sphere_approximation link="link1" />
+ <link_sphere_approximation link="link2" >
+ <sphere center="1.0 2.0 3.0" radius="1.0" />
+ <sphere center="1.0 2.0 4.0" radius="2.0" />
+ </link_sphere_approximation>
+ </robot>
+ '''
+ expected = '''
<robot name="myrobot">
<group name="body">
<joint name="J1" />
@@ -142,82 +142,82 @@ def test_full_srdf(self):
<sphere center="1.0 2.0 4.0" radius="2.0" />
</link_sphere_approximation>
</robot>
- '''
- robot = SRDF.from_xml_string(srdf_data)
- self.assertTrue( xml_matches(robot.to_xml_string(),expected))
+ '''
+ robot = SRDF.from_xml_string(srdf_data)
+ self.assertTrue( xml_matches(robot.to_xml_string(),expected))
def test_simple_srdf(self):
- datadir=rospkg.RosPack().get_path('srdfdom')+"/test/resources/"
- stream = open(datadir+'pr2_desc.1.srdf', 'r')
- robot = SRDF.from_xml_string(stream.read())
- stream.close()
- self.assertTrue(len(robot.virtual_joints)==0)
- self.assertTrue(len(robot.groups)==0)
- self.assertTrue(len(robot.group_states)==0)
- self.assertTrue(len(robot.disable_collisionss)==0)
- self.assertTrue(len(robot.end_effectors)==0)
-
- stream = open(datadir+'pr2_desc.2.srdf', 'r')
- robot = SRDF.from_xml_string(stream.read())
- stream.close()
- self.assertTrue(len(robot.virtual_joints)==1)
- self.assertTrue(len(robot.groups)==1)
- self.assertTrue(len(robot.group_states)==0)
- self.assertTrue(len(robot.disable_collisionss)==0)
- self.assertTrue(len(robot.end_effectors)==0)
+ datadir=rospkg.RosPack().get_path('srdfdom')+"/test/resources/"
+ stream = open(datadir+'pr2_desc.1.srdf', 'r')
+ robot = SRDF.from_xml_string(stream.read())
+ stream.close()
+ self.assertTrue(len(robot.virtual_joints)==0)
+ self.assertTrue(len(robot.groups)==0)
+ self.assertTrue(len(robot.group_states)==0)
+ self.assertTrue(len(robot.disable_collisionss)==0)
+ self.assertTrue(len(robot.end_effectors)==0)
+
+ stream = open(datadir+'pr2_desc.2.srdf', 'r')
+ robot = SRDF.from_xml_string(stream.read())
+ stream.close()
+ self.assertTrue(len(robot.virtual_joints)==1)
+ self.assertTrue(len(robot.groups)==1)
+ self.assertTrue(len(robot.group_states)==0)
+ self.assertTrue(len(robot.disable_collisionss)==0)
+ self.assertTrue(len(robot.end_effectors)==0)
def test_complex_srdf(self):
- datadir=rospkg.RosPack().get_path('srdfdom')+"/test/resources/"
- stream = open(datadir+'pr2_desc.3.srdf', 'r')
- robot = SRDF.from_xml_string(stream.read())
- stream.close()
- self.assertTrue(len(robot.virtual_joints)==1)
- self.assertTrue(len(robot.groups)==7)
- self.assertTrue(len(robot.group_states)==2)
- self.assertTrue(len(robot.disable_collisionss)==2)
- self.assertTrue(robot.disable_collisionss[0].reason=="adjacent")
- self.assertTrue(len(robot.end_effectors)==2)
-
- self.assertTrue(robot.virtual_joints[0].name=="world_joint")
- self.assertTrue(robot.virtual_joints[0].type=="planar")
-
- for group in robot.groups:
- if (group.name == "left_arm" or group.name == "right_arm" ):
- self.assertTrue(len(group.chains)==1)
- if group.name == "arms":
- self.assertTrue(len(group.subgroups)==2)
- if group.name == "base":
- self.assertTrue(len(group.joints)==1)
- if (group.name == "l_end_effector" or group.name == "r_end_effector" ):
- self.assertTrue(len(group.links)==1)
- self.assertTrue(len(group.joints)==9)
- if group.name == "whole_body" :
- self.assertTrue(len(group.joints)==1)
- self.assertTrue(len(group.subgroups)==2)
-
- index=0
- if robot.group_states[0].group !="arms":
- index=1
-
- self.assertTrue(robot.group_states[index].group =="arms")
- self.assertTrue(robot.group_states[index].name =="tuck_arms")
- self.assertTrue(robot.group_states[1-index].group =="base")
- self.assertTrue(robot.group_states[1-index].name =="home")
-
- v=next((joint.value for joint in robot.group_states[index].joints if joint.name=="l_shoulder_pan_joint"),None)
- self.assertTrue(len(v) == 1)
- self.assertTrue(v[0] ==0.2)
-
- w=next((joint.value for joint in robot.group_states[1-index].joints if joint.name=="world_joint"),None)
- self.assertTrue(len(w) == 3)
- self.assertTrue(w[0] ==0.4)
- self.assertTrue(w[1] ==0)
- self.assertTrue(w[2] ==-1)
-
- index = 0 if (robot.end_effectors[0].name[0] == 'r') else 1
- self.assertTrue(robot.end_effectors[index].name == 'r_end_effector')
- self.assertTrue(robot.end_effectors[index].group == 'r_end_effector')
- self.assertTrue(robot.end_effectors[index].parent_link == 'r_wrist_roll_link')
+ datadir=rospkg.RosPack().get_path('srdfdom')+"/test/resources/"
+ stream = open(datadir+'pr2_desc.3.srdf', 'r')
+ robot = SRDF.from_xml_string(stream.read())
+ stream.close()
+ self.assertTrue(len(robot.virtual_joints)==1)
+ self.assertTrue(len(robot.groups)==7)
+ self.assertTrue(len(robot.group_states)==2)
+ self.assertTrue(len(robot.disable_collisionss)==2)
+ self.assertTrue(robot.disable_collisionss[0].reason=="adjacent")
+ self.assertTrue(len(robot.end_effectors)==2)
+
+ self.assertTrue(robot.virtual_joints[0].name=="world_joint")
+ self.assertTrue(robot.virtual_joints[0].type=="planar")
+
+ for group in robot.groups:
+ if (group.name == "left_arm" or group.name == "right_arm" ):
+ self.assertTrue(len(group.chains)==1)
+ if group.name == "arms":
+ self.assertTrue(len(group.subgroups)==2)
+ if group.name == "base":
+ self.assertTrue(len(group.joints)==1)
+ if (group.name == "l_end_effector" or group.name == "r_end_effector" ):
+ self.assertTrue(len(group.links)==1)
+ self.assertTrue(len(group.joints)==9)
+ if group.name == "whole_body" :
+ self.assertTrue(len(group.joints)==1)
+ self.assertTrue(len(group.subgroups)==2)
+
+ index=0
+ if robot.group_states[0].group !="arms":
+ index=1
+
+ self.assertTrue(robot.group_states[index].group =="arms")
+ self.assertTrue(robot.group_states[index].name =="tuck_arms")
+ self.assertTrue(robot.group_states[1-index].group =="base")
+ self.assertTrue(robot.group_states[1-index].name =="home")
+
+ v=next((joint.value for joint in robot.group_states[index].joints if joint.name=="l_shoulder_pan_joint"),None)
+ self.assertTrue(len(v) == 1)
+ self.assertTrue(v[0] ==0.2)
+
+ w=next((joint.value for joint in robot.group_states[1-index].joints if joint.name=="world_joint"),None)
+ self.assertTrue(len(w) == 3)
+ self.assertTrue(w[0] ==0.4)
+ self.assertTrue(w[1] ==0)
+ self.assertTrue(w[2] ==-1)
+
+ index = 0 if (robot.end_effectors[0].name[0] == 'r') else 1
+ self.assertTrue(robot.end_effectors[index].name == 'r_end_effector')
+ self.assertTrue(robot.end_effectors[index].group == 'r_end_effector')
+ self.assertTrue(robot.end_effectors[index].parent_link == 'r_wrist_roll_link')
if __name__ == '__main__':
From d2a2a1207f234aa6519e0196083d682bc24030dd Mon Sep 17 00:00:00 2001
From: Michael Lautman <mike@picknik.ai>
Date: Thu, 14 Mar 2019 15:06:31 -0600
Subject: [PATCH 03/11] adding python generated files to gitignore
---
.gitignore | 2 ++
test/__pycache__/test.cpython-36-PYTEST.pyc | Bin 8285 -> 0 bytes
2 files changed, 2 insertions(+)
delete mode 100644 test/__pycache__/test.cpython-36-PYTEST.pyc
diff --git a/.gitignore b/.gitignore
index 378eac2..443cee8 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1 +1,3 @@
build
+__pycache__
+*.pyc
diff --git a/test/__pycache__/test.cpython-36-PYTEST.pyc b/test/__pycache__/test.cpython-36-PYTEST.pyc
deleted file mode 100644
index 71aea4fc872807796493ae5805515a25829a39bc..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 8285
zcmd5>&2JmW72jPh$t6YV+p;7(4nrhC)D}`~2W^v5j$6x4n-3+8osVuEY*w75L~6;U
zXP34lkmx}MQWQO;haS>X(L;dd&`W_{bM8M-pwmNv_7VX-<XHI9{@(18+$AX|ZiAvE
z=I!pAuQ%^&-n_Y7EapG`^k=vCo=}v(E3u!B_p2!WCjd-gs;ii)d}@tULpRe*Gc)o#
zYi8xsFb(<4nK?XDZoWBXPN~#acMHv;S(NXpJ8jMYmv(2HbLJdy8J4}TnDfkFIXo9w
zo=xGo$O^28=NUH5X7F5kNnx{W?!Ln2oLuAVamrkFR@nT$#y`gQ>al{6)SJrM!U^bA
zwnf(Gtj4_Nnkw|0Z*JYdbE<7~-?<s^dTSS-g<U7GxZ`^rUUjM}b;D1K$~8Rwb%3tY
zP<rY?xvTPLx@toW<oX+F%v|bXM&L3%^^WllR*L!6?X|4P*XrC4uI<%bCXANHoLh%&
zN2Cs#u1J9<p>G2x3fEt~{@V5JTh{jLTi3-@%Q*;c)^}W@P7^7|4TM_#t-@zO#Dn#X
zJ+J9(H0%4$#y#I^dUu_AaB0KmRTR0<jll5(3D(<(VnI@1Rfz=4ah;~q3g&1@1pr0O
z;6JPCYF_1p{jXLJ?!;H0;WP!$5-<ql2;>P&0jz1fNbl1GXlWss#hX|p7Fd4BgFe+O
zs2Qhv9@r|$ihgMnnly#t*C5G2`Ki)Sk5xP&v14^V#Y=(4@CGf>ZYm!rHI=29e%}aE
z{kF&ImU682v=3C&r3ZDge^i(1Y7Lz<tqTjF=fOXze^)!k$Xm*LpY`;v*40;_(<^Ax
zf5Ys5!0g#cbv11XvxZb1z8R#0Ow`I)0UlrBDDrC=7?#`DKU8@J<K>4fhppn{y`$W@
zt1CfvKaU=_RnQB{L;5h@u+M%9_3wU}5r2dc-$jA0PeIoU+ea6+>b07~p<Sg`r@7<s
zl2<F)0c>oi1BJYi&@7VE0?FtZfL&PDd!?QVNkLX!aIm4p-cvR07up^DW~d))y2uYl
z6?#2zU_~vv>4@}Q+wC~87;XJoq1Iv8sy)=H2l<Ly_k*g}Y+rebrY4=qC`ZrOu4@fP
zw3>EM-CM7?Ue$K}D|qR5`pOo-Y2|#8z!?Hd05FEL^uA2*Abe*?I9~<sX_Sa!P7dnm
z$sxN3#FvN-{tKn2{F2>bc$`qv-CzFc>CdLt=dK(*HK~teTD;S(HAAFarzOc0={B5X
zAky4!?K(pDJsyaZz2l2?&Gqa+<liQyU+3K8YiUj<%Fh9q8Jn2uo0%Q?lnaw<!b%6N
zcxddCB=7_Vk=miCi>d(!bsm2M|9N#;E#X^JzlNs)=e3IGw921EkAWi~;nKz>o%hFh
z>B2AY*{({Ewg>w5&&%%;IRbL-aNr6|bSr>M=zqt}2sjOGG_=`68@>M>Zxf&=R9N%x
z(x`zRq`PFkX;N!oe;#2o5}S#zzagyLw9?zRZX7*6loM?kr2u;dNr;7FWk=;<gUF+g
ziGD2`I^GLdd${v#Lpdaq#B|gNy9X(|bo`wBXhJqgm&{26zlGwL0Ti_W5rkz?z7fo$
z%tuAGpDd2wp+9`35`ZCmHPDD<uz8)>29|++vgWx!Lz@(_2tGCx5xim}+b|kA?744H
zeZLpP60dWq4mjB_$VVdiB$x_4Kl&AdVq=>8ZGgxGc#WkNK?VGxI;>;xIxG$Umf1ck
zzDB$%)$F?KuooVvn<v{?$BkQSGa_I2>n%U9TUBiBe!#^P_V<vtj?YOA#SHVR9ol-Y
zhdc13T~QRbUcdI%AP$fm<e~iTarikIH28<U$oK*CI>E!F^>s}8GKyaUP#~FES!N+K
zDtQ=AC_RFlT34xI|3~I?6~+GsfFt8QQ)4RA5PzlQeyNx`_Qnjp(=5vjJToi@PUXe)
zErcHwXuUx}79Y4x;={lsX<VlEFk(*&BCp>gqa$s=p2||kDt1%sr`QoGn$oxJ7JBDP
z(XYb29S>U#lzOw=Jd|JME5qtt?seKx^^V65V|ArcrQyM;MYS(qESENhJx6OVJ+$`n
zXl-@RuD41%w(nSo-}X1lk}``B<x)^@C$RV)?H{Mx=FLQPWkc2uX|2d_3#u15(Q1wy
z?v+atzgZqx_}H?g@82vxo17)V94!A!v;rF@w+bl^DU8wWv=ECTT&M;fj|eB;IN%XN
zbC^lQCdr^^F_IfmZG3%k;Z-IK>;-!cpE!EIX27p8hTpC8pkupMoFxeyWD4iJS(ck2
z;`IUjH7+%iBw)$+aw!bKL#-H_iPe33$91f#=OW^$do902EiZ1C<)bXWE`?u7rK?Qn
z${pMHTAStXvW8s+JIkfhM@uR=;+@vM<+s5z$FkdP?j6*zOMoCb!hoEZ#eAtQm}Mw`
zvf)ZNM~TRD_-6Uy`m?1=D3{lvj@)MTjxUi3-F#Tb=i(hPQlevJ<J5Hx$*OEfV;v%F
zfrCW;R96$4-?)<*v$Kd<N&JxSN&HZ{l}C0a{fVS~M1&1hE#^NS!E+?3gR>m7^kj=k
z9wXUe+*?GH4gEyYkmDX8^!Ow8obU%@9z=2_Ibt|U-vNv}o_@^)*HB3~fN`r1hfXwl
zm{%C}AE7&&WYk~i4<4x##5C?DLQb7(Did75qZ$7r8~te=PBHd}n)#S{-@u85G6iC$
z#=WLRK^vkscySyteS|K-E#7ftaAXz(FIJC3sO~#%&CF5C#bS0~3&S~RV?T2k&Po)T
zga<eoM9WjoG-YM1S_h|1>hwP5^(iZ&tm>;2Iv+ijjGNYz?FV6023@GglMSWh0t8YN
zFyGdCNLqwRsjkK}I>j*kINeL(^pRreW38uC_}tYS#xX+OUK(#XnZ!^U`CzJ>ZWNGK
z$n49Y3!zwuD^9>6a2{(nGXXa{#K|BoyDf~iUEQ~L9ULJ2J#6(N*H66pM>B1H$zqOQ
zUB9?aJQ6CGhuyU~P6-2%dgGOsgy#7|Z`;A1&}ml|y4S{8mBh#|Q6n8%4H;Bj4-&jg
z=*I{>oiIqm(9&i2$o)~E7&;zbER64^z9`0n3V$ubpQ8a)OBABDSdyG{jVzynb5PLa
z-=wLNXDBf%W$M?PZS3^4!jI6%r}Mvp6Kn=0k7D4&8~%?TpQID9{{MGcksZ+<(uz?W
zX+?yST48MSZxgegC-4G5Oc5?gMd06|s`CUWqY>!?uMlFBz!d^761YlWpbh-H^!Ysk
z16AN;Ied%2bpkgCyaW(QmVciRKOjISDE<n8)1}#T5UMn#G(SemN0w%+|3I2~3^cIu
z+gQ#2z{W?VL^f_rkY)@Ak1@k?D@tRQ<wsi1C3}-^kuB%h)Uf3MyTA$|_TV4D6_G&w
zTQAeiH0D`xU;9w?3-W!M-jQk#JyWK!h&lS+irUV|`m2NbGcg*depc4MKd4_C=|3mu
zI*@bWIstS2deHW4tZjdO>c0@JhQ?n+QsVPLkND{Q9x;mUe2&y<u9wAnrzK~WK|iHm
zKW7<W(SvHbS#XzX20nR!oo8o~SeEH#mlfF_E0w8?Xp}+CJX;EDf)#d_E#FrLqb*U&
zn<>~MjA#W$a>ZR-${?M+GU&hB?>`uQI7TiSBZo20#c0XNG0rD2NRT6yJ)}3skdkH>
zB22n57wHYnj;odrQ+%G~T3U=tbCOH*bOjV!b%<S_h+Somg>9w(dHf;%=kkdEIicH(
z6H1RrPb3UGI$?>admhIPi##csnYL@UY;GE^QwuCa;AW23clW|q9pBi)C?#M{yH<R6
zF{gRr`-HAc%>2DQ4;u!ZCCyyWsqWK=7=bQ=20nvmBERQTp{x;->PtPO(;G;jKr<<G
zslJWG{~aW9kWkUgMMNTdUdLq)#}3vCN#90RX*4S>(sETJyV-+WW!J$yO`Bi&3(ZLc
z=*rRMNm~|W-CgHEddXOBz2ATLS0B;uk1zh`tKmPy^pl9^Z=qs@bpAGQe?^IiJ|V_M
zMEv<hbB<d`xFK5OIE>3o#L2(#TRSJikgsst5t%Rqm)nnIyU^)8U}ogW&qsXO*$D#`
zGb3f-n`w&95j{yXZa#2PX3~9TKWVa-m+{HzyV#GhR*|l^m~$Z1yF$G;!W=Otqm;PF
zeow5r4KO)OSyC6ZC3Q(FAT_^;ykc1BeGvi8qPncfyfC#vvVK)Nk1X-LRw5|hE&#uz
z9+kdGbLc|Vuq+C*EKB4qTz;|+3<HsvW!*uZCamF<rsl5_c#XjO1cs5=ASCA(QSly%
zjKhtT@w`zqiV{oHG?B=&thIB(*y+^W02y9UxY|Cn245>y`;b_*9v&8uB3b8-%+!k9
z041`0TppbzXKfx+p$*J0j83Z_P@P$zs1O&!q1z#~6f-!NtP`qPZ`JV(6Ux~LD{ofK
z^I!>*W>Tm`)t;m!ywLVTt)<&tndFyu0AX^O^1i|#wqJuYk}Kyn;dEO3B%TSQ5;FaY
k^fgbSv4NWuU8AcR$|)CALo1{W`Yjm?#=`thlo><)H^D~}Z~y=R
From 3daf790e1ef036d5db2007233dff47dcf73b84c0 Mon Sep 17 00:00:00 2001
From: Michael Lautman <mike@picknik.ai>
Date: Thu, 14 Mar 2019 15:07:45 -0600
Subject: [PATCH 04/11] updating cmake and package.xml for ros2
---
CMakeLists.txt | 93 ++++++++++++++++++++++++++++++++------------------
package.xml | 29 +++++++++++-----
2 files changed, 79 insertions(+), 43 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 620e34c..c0b1d7d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,66 +1,91 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
project(srdfdom)
-find_package(Boost REQUIRED)
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 14)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(console_bridge_vendor REQUIRED) # Provides console_bridge 0.4.0 on platforms without it.
find_package(console_bridge REQUIRED)
find_package(urdfdom_headers REQUIRED)
+find_package(urdf REQUIRED)
+find_package(tinyxml_vendor REQUIRED)
+find_package(TinyXML REQUIRED) # provided by tinyxml_vendor
+find_package(ament_index_cpp REQUIRED)
-find_package(catkin REQUIRED COMPONENTS cmake_modules urdf urdfdom_py)
-
-find_package(TinyXML REQUIRED)
+find_package(Boost REQUIRED)
include_directories(
include
+ ${rclcpp_INCLUDE_DIRS}
+ ${rmw_implementation_INCLUDE_DIRS}
${Boost_INCLUDE_DIR}
${TinyXML_INCLUDE_DIRS}
- ${catkin_INCLUDE_DIRS}
${console_bridge_INCLUDE_DIRS}
${urdfdom_headers_INCLUDE_DIRS}
- )
-
-add_compile_options(-std=c++11)
-
-catkin_python_setup()
-
-catkin_package(
- LIBRARIES ${PROJECT_NAME}
- INCLUDE_DIRS include
- DEPENDS TinyXML console_bridge urdfdom_headers
+ ${std_msgs_INCLUDE_DIRS}
)
+ament_python_install_package(${PROJECT_NAME}
+ PACKAGE_DIR src/${PROJECT_NAME})
+
add_library(${PROJECT_NAME}
src/model.cpp
src/srdf_writer.cpp
)
-target_link_libraries(${PROJECT_NAME} ${TinyXML_LIBRARIES} ${catkin_LIBRARIES} ${console_bridge_LIBRARIES} ${urdfdom_headers_LIBRARIES})
+ament_target_dependencies(${PROJECT_NAME}
+ ${TinyXML_LIBRARIES}
+ ${console_bridge_LIBRARIES}
+ ${urdfdom_headers_LIBRARIES}
+ ${urdf_LIBRARIES}
+)
-install(TARGETS ${PROJECT_NAME}
- DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+install(
+ TARGETS
+ ${PROJECT_NAME}
+ DESTINATION lib
)
+
install(DIRECTORY include/${PROJECT_NAME}/
- DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
- FILES_MATCHING PATTERN "*.h"
+ DESTINATION include/${PROJECT_NAME}
+)
+
+# Mark resources for installation
+install(DIRECTORY test/resources
+ DESTINATION share/${PROJECT_NAME}
)
install(PROGRAMS
scripts/display_srdf
- DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+ DESTINATION lib/${PROJECT_NAME}
)
-if(CATKIN_ENABLE_TESTING)
- find_package(rostest REQUIRED)
- add_rostest(test/srdf_parser.test)
+if(BUILD_TESTING)
+ find_package(ament_cmake_pytest REQUIRED)
+ find_package(ament_cmake_gtest REQUIRED)
- add_definitions(-DTEST_RESOURCE_LOCATION="${CMAKE_SOURCE_DIR}/test/resources")
- execute_process(COMMAND bash -c "locale -a | grep -q ^nl_NL"
- RESULT_VARIABLE TEST_LOCALE
- OUTPUT_QUIET ERROR_QUIET)
- if (TEST_LOCALE)
- message(WARNING "Locale nl_NL not available. Locale test will not be meaningful.")
- endif()
+ ament_add_gtest(test_parser test/test_parser.cpp)
+ target_link_libraries(test_parser
+ ${PROJECT_NAME}
+ ${rclcpp_LIBRARIES}
+ ${TinyXML_LIBRARIES}
+ ${console_bridge_LIBRARIES}
+ ${urdfdom_headers_LIBRARIES}
+ ${urdf_LIBRARIES}
+ ament_index_cpp::ament_index_cpp
+ )
- add_rostest_gtest(test_cpp test/srdf_parser_cpp.test test/test_parser.cpp)
- target_link_libraries(test_cpp ${catkin_LIBRARIES} ${PROJECT_NAME})
+ # python tests with python interfaces of message filters
+ find_package(ament_cmake_pytest REQUIRED)
+ ament_add_pytest_test(test.py "test/test.py"
+ APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}
+ TIMEOUT 90)
endif()
+
+ament_package()
\ No newline at end of file
diff --git a/package.xml b/package.xml
index 1d57277..3c38a19 100644
--- a/package.xml
+++ b/package.xml
@@ -1,4 +1,6 @@
-<package>
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
<name>srdfdom</name>
<version>0.5.1</version>
<description>Parser for Semantic Robot Description Format (SRDF).</description>
@@ -11,22 +13,31 @@
<url type="bugtracker">https://github.com/ros-planning/srdfdom/issues</url>
<url type="repository">https://github.com/ros-planning/srdfdom</url>
- <buildtool_depend>catkin</buildtool_depend>
+ <buildtool_depend>ament_cmake</buildtool_depend>
+
+ <buildtool_depend>python_cmake_module</buildtool_depend>
+
+ <depend>console_bridge_vendor</depend>
<build_depend>boost</build_depend>
- <build_depend>cmake_modules</build_depend>
+ <build_depend>python_cmake_module</build_depend>
<build_depend>libconsole-bridge-dev</build_depend>
<build_depend>urdf</build_depend>
<build_depend>liburdfdom-headers-dev</build_depend>
<build_depend>urdfdom_py</build_depend>
<build_depend>tinyxml</build_depend>
- <run_depend>boost</run_depend>
- <run_depend>libconsole-bridge-dev</run_depend>
- <run_depend>liburdfdom-headers-dev</run_depend>
- <run_depend>tinyxml</run_depend>
- <run_depend>urdfdom_py</run_depend>
+ <exec_depend>boost</exec_depend>
+ <exec_depend>libconsole-bridge-dev</exec_depend>
+ <exec_depend>liburdfdom-headers-dev</exec_depend>
+ <exec_depend>tinyxml</exec_depend>
+ <exec_depend>urdfdom_py</exec_depend>
- <test_depend>rostest</test_depend>
+ <test_depend>ament_index_cpp</test_depend>
+ <test_depend>ament_cmake_pytest</test_depend>
+ <test_depend>python3-pytest</test_depend>
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
</package>
From 854982609494709b775f8c1364245fcaacfe3506 Mon Sep 17 00:00:00 2001
From: Michael Lautman <mike@picknik.ai>
Date: Thu, 14 Mar 2019 15:11:45 -0600
Subject: [PATCH 05/11] porting test.py to ros2
---
test/test.py | 34 +++++++++++++++++++++++-----------
1 file changed, 23 insertions(+), 11 deletions(-)
diff --git a/test/test.py b/test/test.py
index 2947335..7e0d723 100755
--- a/test/test.py
+++ b/test/test.py
@@ -1,13 +1,16 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
+
PKG = 'srdfdom'
+import os
import sys
-import rospkg
import unittest
from srdfdom.srdf import SRDF
from xml.dom.minidom import parseString
import xml.dom
+from ament_index_python.resources import get_resource
+
# xml match code from test_xacro.py
# by Stuart Glaser and William Woodall
@@ -88,8 +91,8 @@ def xml_matches(a, b):
## A python unit test for srdf
class TestSRDFParser(unittest.TestCase):
- ## test valid srdf
+ ## test valid srdf
def test_full_srdf(self):
srdf_data = '''
<robot name="myrobot">
@@ -117,6 +120,7 @@ def test_full_srdf(self):
</link_sphere_approximation>
</robot>
'''
+
expected = '''
<robot name="myrobot">
<group name="body">
@@ -144,11 +148,14 @@ def test_full_srdf(self):
</robot>
'''
robot = SRDF.from_xml_string(srdf_data)
- self.assertTrue( xml_matches(robot.to_xml_string(),expected))
+ self.assertTrue( xml_matches(robot.to_xml_string(False),expected))
+
def test_simple_srdf(self):
- datadir=rospkg.RosPack().get_path('srdfdom')+"/test/resources/"
- stream = open(datadir+'pr2_desc.1.srdf', 'r')
+ _, package_path = get_resource("packages", PKG)
+ fname = os.path.join(
+ package_path, "share", PKG, "resources", "pr2_desc.1.srdf")
+ stream = open(fname, 'r')
robot = SRDF.from_xml_string(stream.read())
stream.close()
self.assertTrue(len(robot.virtual_joints)==0)
@@ -157,7 +164,11 @@ def test_simple_srdf(self):
self.assertTrue(len(robot.disable_collisionss)==0)
self.assertTrue(len(robot.end_effectors)==0)
- stream = open(datadir+'pr2_desc.2.srdf', 'r')
+ def test_medium_srdf(self):
+ _, package_path = get_resource("packages", PKG)
+ fname = os.path.join(
+ package_path, "share", PKG, "resources", "pr2_desc.2.srdf")
+ stream = open(fname, 'r')
robot = SRDF.from_xml_string(stream.read())
stream.close()
self.assertTrue(len(robot.virtual_joints)==1)
@@ -167,8 +178,10 @@ def test_simple_srdf(self):
self.assertTrue(len(robot.end_effectors)==0)
def test_complex_srdf(self):
- datadir=rospkg.RosPack().get_path('srdfdom')+"/test/resources/"
- stream = open(datadir+'pr2_desc.3.srdf', 'r')
+ _, package_path = get_resource("packages", PKG)
+ fname = os.path.join(
+ package_path, "share", PKG, "resources", "pr2_desc.3.srdf")
+ stream = open(fname, 'r')
robot = SRDF.from_xml_string(stream.read())
stream.close()
self.assertTrue(len(robot.virtual_joints)==1)
@@ -221,5 +234,4 @@ def test_complex_srdf(self):
if __name__ == '__main__':
- import rostest
- rostest.rosrun(PKG, 'srdf_python_parser_test', TestSRDFParser)
+ unittest.main()
\ No newline at end of file
From 47ef93e68d3acbf92c79b1c105771306d802c3a3 Mon Sep 17 00:00:00 2001
From: Michael Lautman <mike@picknik.ai>
Date: Thu, 14 Mar 2019 15:24:35 -0600
Subject: [PATCH 06/11] removing unused .test files
---
test/srdf_parser.test | 3 ---
test/srdf_parser_cpp.test | 8 --------
2 files changed, 11 deletions(-)
delete mode 100644 test/srdf_parser.test
delete mode 100644 test/srdf_parser_cpp.test
diff --git a/test/srdf_parser.test b/test/srdf_parser.test
deleted file mode 100644
index e40ee04..0000000
--- a/test/srdf_parser.test
+++ /dev/null
@@ -1,3 +0,0 @@
-<launch>
- <test test-name="test_srdf_parser_python" pkg="srdfdom" type="test.py" />
-</launch>
diff --git a/test/srdf_parser_cpp.test b/test/srdf_parser_cpp.test
deleted file mode 100644
index d7a3ce3..0000000
--- a/test/srdf_parser_cpp.test
+++ /dev/null
@@ -1,8 +0,0 @@
-<launch>
- <test test-name="test_srdf_parser_cpp" pkg="srdfdom" type="test_cpp">
- <env name="LC_ALL" value="C" />
- </test>
- <test test-name="test_srdf_parser_cpp_locale" pkg="srdfdom" type="test_cpp">
- <env name="LC_ALL" value="nl_NL.UTF-8" />
- </test>
-</launch>
From f32c5e9f4e974ffbf210797078df9b1431b54258 Mon Sep 17 00:00:00 2001
From: Michael Lautman <mike@picknik.ai>
Date: Thu, 14 Mar 2019 15:25:16 -0600
Subject: [PATCH 07/11] porting cpp tests to ros2
---
test/test_parser.cpp | 67 +++++++++++++++++++++++++++++++++-----------
1 file changed, 50 insertions(+), 17 deletions(-)
diff --git a/test/test_parser.cpp b/test/test_parser.cpp
index abe87cb..95a0a49 100644
--- a/test/test_parser.cpp
+++ b/test/test_parser.cpp
@@ -36,17 +36,15 @@
#include <srdfdom/model.h>
#include <urdf_parser/urdf_parser.h>
+#include <ament_index_cpp/get_resource.hpp>
+
#include <fstream>
#include <stdexcept>
#include <gtest/gtest.h>
-#ifndef TEST_RESOURCE_LOCATION
-#define TEST_RESOURCE_LOCATION "."
-#endif
-
struct ScopedLocale
{
- ScopedLocale(const char* name = "C")
+ ScopedLocale(const char* name)
{
backup_ = setlocale(LC_ALL, nullptr); // store current locale
setlocale(LC_ALL, name);
@@ -58,9 +56,9 @@ struct ScopedLocale
std::string backup_;
};
-urdf::ModelInterfaceSharedPtr loadURDF(const std::string& filename)
+urdf::ModelInterfaceSharedPtr loadURDF(const std::string& filename, const char* locale_name)
{
- ScopedLocale l("C");
+ ScopedLocale l(locale_name);
// get the entire file
std::string xml_string;
std::fstream xml_file(filename.c_str(), std::fstream::in);
@@ -82,27 +80,34 @@ urdf::ModelInterfaceSharedPtr loadURDF(const std::string& filename)
}
}
-TEST(TestCpp, testSimple)
+void testSimple(const char* locale_name)
{
+ std::string package_name = "srdfdom";
srdf::Model s;
- urdf::ModelInterfaceSharedPtr u = loadURDF(std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.urdf");
+ std::string content;
+ std::string prefix_path;
+ EXPECT_TRUE(ament_index_cpp::get_resource("packages", package_name, content, &prefix_path));
+ std::string test_resource_location = prefix_path + "/share/" + package_name + "/resources/";
+
+ setlocale(LC_ALL, locale_name);
+ urdf::ModelInterfaceSharedPtr u = loadURDF(test_resource_location + "/pr2_desc.urdf", locale_name);
ASSERT_TRUE(u != NULL);
- EXPECT_TRUE(s.initFile(*u, std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.1.srdf"));
+ EXPECT_TRUE(s.initFile(*u, test_resource_location + "/pr2_desc.1.srdf"));
EXPECT_TRUE(s.getVirtualJoints().size() == 0);
EXPECT_TRUE(s.getGroups().size() == 0);
EXPECT_TRUE(s.getGroupStates().size() == 0);
EXPECT_TRUE(s.getDisabledCollisionPairs().empty());
EXPECT_TRUE(s.getEndEffectors().size() == 0);
- EXPECT_TRUE(s.initFile(*u, std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.2.srdf"));
+ EXPECT_TRUE(s.initFile(*u, test_resource_location + "/pr2_desc.2.srdf"));
EXPECT_TRUE(s.getVirtualJoints().size() == 1);
EXPECT_TRUE(s.getGroups().size() == 1);
EXPECT_TRUE(s.getGroupStates().size() == 0);
EXPECT_TRUE(s.getDisabledCollisionPairs().empty());
EXPECT_TRUE(s.getEndEffectors().size() == 0);
- EXPECT_TRUE(s.initFile(*u, std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.1.srdf"));
+ EXPECT_TRUE(s.initFile(*u, test_resource_location + "/pr2_desc.1.srdf"));
EXPECT_TRUE(s.getVirtualJoints().size() == 0);
EXPECT_TRUE(s.getGroups().size() == 0);
EXPECT_TRUE(s.getGroupStates().size() == 0);
@@ -110,13 +115,31 @@ TEST(TestCpp, testSimple)
EXPECT_TRUE(s.getEndEffectors().size() == 0);
}
-TEST(TestCpp, testComplex)
+TEST(TestCpp, testSimpleC)
+{
+ testSimple("C");
+}
+
+TEST(TestCpp, testSimpleUTF)
+{
+ testSimple("nl_NL.UTF-8");
+}
+
+void testComplex(const char* locale_name)
{
srdf::Model s;
- urdf::ModelInterfaceSharedPtr u = loadURDF(std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.urdf");
+
+ std::string package_name = "srdfdom";
+ std::string content;
+ std::string prefix_path;
+ EXPECT_TRUE(ament_index_cpp::get_resource("packages", package_name, content, &prefix_path));
+ std::string test_resource_location = prefix_path + "/share/" + package_name + "/resources/";
+
+ setlocale(LC_ALL, locale_name);
+ urdf::ModelInterfaceSharedPtr u = loadURDF(test_resource_location + "/pr2_desc.urdf", locale_name);
EXPECT_TRUE(u != NULL);
- EXPECT_TRUE(s.initFile(*u, std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.3.srdf"));
+ EXPECT_TRUE(s.initFile(*u, test_resource_location + "/pr2_desc.3.srdf"));
EXPECT_TRUE(s.getVirtualJoints().size() == 1);
EXPECT_TRUE(s.getGroups().size() == 7);
EXPECT_TRUE(s.getGroupStates().size() == 2);
@@ -167,13 +190,23 @@ TEST(TestCpp, testComplex)
EXPECT_TRUE(s.getEndEffectors()[index].name_ == "r_end_effector");
EXPECT_TRUE(s.getEndEffectors()[index].component_group_ == "r_end_effector");
EXPECT_TRUE(s.getEndEffectors()[index].parent_link_ == "r_wrist_roll_link");
+
+}
+
+TEST(TestCpp, testComplexC)
+{
+ testComplex("C");
+}
+
+
+TEST(TestCpp, testComplexUTF)
+{
+ testComplex("nl_NL.UTF-8");
}
int main(int argc, char** argv)
{
// use the environment locale so that the unit test can be repeated with various locales easily
- setlocale(LC_ALL, "");
- std::cout << "Using locale: " << setlocale(LC_ALL, nullptr) << std::endl;
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
From efc6f3928eef1c2fe4323b48da04bbe2876097cb Mon Sep 17 00:00:00 2001
From: Michael Lautman <mike@picknik.ai>
Date: Thu, 14 Mar 2019 16:05:31 -0600
Subject: [PATCH 08/11] updating travis.yml to crystal
---
.travis.yml | 8 ++++----
1 file changed, 4 insertions(+), 4 deletions(-)
diff --git a/.travis.yml b/.travis.yml
index 3bd4e26..23a2e3c 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -15,14 +15,14 @@ env:
global:
# generate "exotic" locale for parser test
- BEFORE_SCRIPT="sudo apt-get -qq install -y locales; sudo locale-gen nl_NL.UTF-8"
- - ROS_DISTRO=melodic
+ - ROS_DISTRO=crystal
- ROS_REPO=ros
matrix:
- - TEST="clang-format, catkin_lint"
- - ROS_DISTRO=melodic
+ - TEST="clang-format"
+ - ROS_DISTRO=crystal
before_script:
- - git clone -q --depth=1 https://github.com/ros-planning/moveit_ci.git .moveit_ci
+ - git clone -q --depth=1 -b ros2 https://github.com/ros-planning/moveit_ci.git .moveit_ci
script:
- .moveit_ci/travis.sh
From e8d055c39f73d73dcae1f2c2169cf4a64160c010 Mon Sep 17 00:00:00 2001
From: Mike Lautman <mikeblautman@github.com>
Date: Mon, 25 Mar 2019 13:56:50 -0600
Subject: [PATCH 09/11] adding braces to satisfy -Wdangling-else
---
test/test_parser.cpp | 6 ++++++
1 file changed, 6 insertions(+)
diff --git a/test/test_parser.cpp b/test/test_parser.cpp
index 95a0a49..ee2cedc 100644
--- a/test/test_parser.cpp
+++ b/test/test_parser.cpp
@@ -152,11 +152,17 @@ void testComplex(const char* locale_name)
for (std::size_t i = 0; i < s.getGroups().size(); ++i)
{
if (s.getGroups()[i].name_ == "left_arm" || s.getGroups()[i].name_ == "right_arm")
+ {
EXPECT_TRUE(s.getGroups()[i].chains_.size() == 1);
+ }
if (s.getGroups()[i].name_ == "arms")
+ {
EXPECT_TRUE(s.getGroups()[i].subgroups_.size() == 2);
+ }
if (s.getGroups()[i].name_ == "base")
+ {
EXPECT_TRUE(s.getGroups()[i].joints_.size() == 1);
+ }
if (s.getGroups()[i].name_ == "l_end_effector" || s.getGroups()[i].name_ == "r_end_effector")
{
EXPECT_TRUE(s.getGroups()[i].links_.size() == 1);
From dde19895a780d33378492383277be3b0604fbd13 Mon Sep 17 00:00:00 2001
From: Mike Lautman <mikeblautman@github.com>
Date: Thu, 28 Mar 2019 15:22:38 -0600
Subject: [PATCH 10/11] clang
---
test/test_parser.cpp | 2 --
1 file changed, 2 deletions(-)
diff --git a/test/test_parser.cpp b/test/test_parser.cpp
index ee2cedc..50b40d1 100644
--- a/test/test_parser.cpp
+++ b/test/test_parser.cpp
@@ -196,7 +196,6 @@ void testComplex(const char* locale_name)
EXPECT_TRUE(s.getEndEffectors()[index].name_ == "r_end_effector");
EXPECT_TRUE(s.getEndEffectors()[index].component_group_ == "r_end_effector");
EXPECT_TRUE(s.getEndEffectors()[index].parent_link_ == "r_wrist_roll_link");
-
}
TEST(TestCpp, testComplexC)
@@ -204,7 +203,6 @@ TEST(TestCpp, testComplexC)
testComplex("C");
}
-
TEST(TestCpp, testComplexUTF)
{
testComplex("nl_NL.UTF-8");
From eb9adfa6d7375c1094f64db4be7ad0b61dbcf040 Mon Sep 17 00:00:00 2001
From: Mike Lautman <mikeblautman@github.com>
Date: Thu, 28 Mar 2019 15:23:28 -0600
Subject: [PATCH 11/11] pointing moveit_ci to the correct branch
---
.travis.yml | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/.travis.yml b/.travis.yml
index 23a2e3c..abf8557 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -22,7 +22,7 @@ env:
- ROS_DISTRO=crystal
before_script:
- - git clone -q --depth=1 -b ros2 https://github.com/ros-planning/moveit_ci.git .moveit_ci
+ - git clone -q -b ros2 --depth=1 -b ros2 https://github.com/ros-planning/moveit_ci.git .moveit_ci
script:
- .moveit_ci/travis.sh