From 7f50e350ba266b4d57194de22438801226472512 Mon Sep 17 00:00:00 2001 From: Max Hunt Date: Tue, 11 Feb 2020 12:11:39 +0000 Subject: [PATCH] Update --- bricks_etc.py | 2 +- main.py | 17 ++++++++++++--- models_NEEDTOMOVE/empty/meshes/Brick.STL | Bin 0 -> 12084 bytes models_NEEDTOMOVE/empty/meshes/Brick_1.STL | Bin 0 -> 4884 bytes models_NEEDTOMOVE/empty/model.config | 19 ++++++++++++++++ models_NEEDTOMOVE/empty/model.sdf | 24 +++++++++++++++++++++ 6 files changed, 58 insertions(+), 4 deletions(-) create mode 100755 models_NEEDTOMOVE/empty/meshes/Brick.STL create mode 100755 models_NEEDTOMOVE/empty/meshes/Brick_1.STL create mode 100755 models_NEEDTOMOVE/empty/model.config create mode 100755 models_NEEDTOMOVE/empty/model.sdf diff --git a/bricks_etc.py b/bricks_etc.py index d3dfb0e..ec0bf36 100644 --- a/bricks_etc.py +++ b/bricks_etc.py @@ -11,7 +11,7 @@ table = { 'yaw':radians(90) } -bricks_end =[{ #DO NOT USE THIS +bricks_end =[{ 'id':'ea1', 'rframe':'t1', 'x':0.46, diff --git a/main.py b/main.py index 1afd3bf..08c5206 100755 --- a/main.py +++ b/main.py @@ -28,7 +28,9 @@ running = True bricks = bet.getBuildable() table = bet.getTable() -brick_targets = bet.getTargets() +brick_targets = bet.getDirections() +built_bricks = bet.getTargets() + class PickAndPlace(object): @@ -164,7 +166,6 @@ def _tf_service_initializer(): tf_service = ots.init() - def tf_service(init=True): global tf_service_thread if init: @@ -176,7 +177,6 @@ def tf_service(init=True): ots.SIGKILL = True - def cleanup(): delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel) for obj in bet.getAll(): @@ -187,6 +187,7 @@ def load_objects(): # Load Table SDFs with open ("models/L3-table/model.sdf", "r") as table_file:table_xml=table_file.read().replace('\n', '') with open ("models/brick/model.sdf", "r") as brick_file:brick_xml=brick_file.read().replace('\n', '') + with open ("models/empty/model.sdf", "r") as empty_file:empty_xml=empty_file.read().replace('\n', '') # Spawn Table SDF rospy.wait_for_service('/gazebo/spawn_sdf_model') spawn_sdf = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel) @@ -207,6 +208,16 @@ def load_objects(): brick_reference_frame=brick['rframe'] brick_id = brick['id'] brick_ents.append(spawn_sdf(brick_id, brick_xml, "/", brick_pose, brick_reference_frame)) + + for brick in built_bricks: + ber = [brick['roll'], brick['pitch'], brick['yaw']] #brick_euler_rotation + brick_pose = Pose(position=Point(x=brick['x'], y=brick['y'], z=brick['z'])) + brick_pose.position = Point(x=brick['x'], y=brick['y'], z=brick['z']) + bqo = quaternion_from_euler(brick['roll'], brick['pitch'], brick['yaw']) + brick_pose.orientation = Quaternion(bqo[0], bqo[1], bqo[2], bqo[3]) + brick_reference_frame=brick['rframe'] + brick_id = brick['id'] + brick_ents.append(spawn_sdf(brick_id, empty_xml, "/", brick_pose, brick_reference_frame)) #TODO: delete this line diff --git a/models_NEEDTOMOVE/empty/meshes/Brick.STL b/models_NEEDTOMOVE/empty/meshes/Brick.STL new file mode 100755 index 0000000000000000000000000000000000000000..928d130b2e210df44a6bb855d53c6cd9d02c1762 GIT binary patch literal 12084 zcmb`Nzl&W}5XawIq)2S+(kKB3HpKg1l|E?YS_UNbQzFU6t&+D`AkK8@Oc&V{;^j!bfL`X&mC#9p$_T02)yV#ID zpF3<6{UG#H^2$Ao=lXwpu79r`I;M4m4f!5p+HkLGLbTZ!BiuXb=(D}Dc_UUNH1@eQ zQ_(H|7DD4>!|8ZlbYm^_{rAhS`Z|stI5UOq!K<ZgjN`#{I9izTDKBVh>_{~jZ6ym|hdujAk8J)4bJ!^)|J^`#x8gapQmQ5#EY z!NaujXK&0sR!0wPbay`Sh>tD%_Q)%GMFQEAY>=RqJ$HNBr)C|4gam4^wJ{LX;{Mk* zm<<*mrxk_$c}2n^?X>NAyB~vB)Pjw+K7yE$z_`t;-H1(iIIUNH;lX>Cst@^#@ut=M zLeHb-MU}IJ5)zg@K9j{Q*&va%qULL_^ixPkSdO`k_Dm!}EuIp!4fwto@i65TJn2Yu zSB`e_w)Khxwf;Qyj1z6FOl#WD)tQIBn>s(tUYxZ3QOZP0I4{<;2SF{IM2Vn;1fsqNK`rcTBG~oO z`}TQKlR$lRb$z6h+Uw3v17?0AP!k>JMLq68P|Kp=^`Y)ldu0)IiSoyG(8sC|R$5I^ zLIUGdA0xzq9@aJ};k@uRt%C%$##hehb|2kwdw)LebI(a2uaZ~Xb84Z|5tA+-td&+`pLBXhODu2 zSL@TbwLyuhD+t{NHLfzzy)t4ZiOn?wJyZSbdDS_c&>l!4uRt#qx1Lx>H&#AKNa&Oh z(RxMey8B2=u>$SS^t{B15)!&CNvmB432I?C-Mh67orC&GdrseBFRh#s60p70jQR|F z9;>aQ3lEbGN;ogpkqBmk<*|F9H5u*Wz4MBM-Qk?5I%nk7Fc-C8V^-E3E2o46#-2s% zSd9?6C%SE&uC)zH5C!E`c)N#JBrNJq?3!1@T-35$@aM|o+AHjo-qqX#`EFkMShZOb zl#sAkxu$x_I6*DUv>YY$<~7WV@nT=Z0v@JJWPPCKJ62k4gAx+fiQL2XEFnQH>u|mg zZSL}eM>&iHc7rH-a?l!uVkkEZagia#;uTMD>Nl?pn)-_v#5)xcru@E*$Pz!sW zyrP7}c&tpzcTG8}8;3lQ@8g+B2?^axMDH5iX-KGrnbOHX2?>n%M{J_f+z0tlc90Uz zi=LPWR8)0CLalfN);1_XtyFg{Y3noG8J7A<=WDu=`P}W3v zkQk2@32Gr@k_}2oaBmbDecBqdjOipvHJ`G?=JM!Div%hyc}0R+=B>}9o+!Ke6t5^D zVQX>^+mo6EwJe5iqa`RI!5o`R{3L)oZX=$6QY%GlgZ)t-f-gV zV=pdAXdGI2zNm;9C7I}HBjDB62(iHWEF)v(KOMZZMG0&;(MQ1uM0ZXCE3e)~CKK%7 z9DD9}s+bphV>XZ_6+tazs1eA-IVG8pJ1GgXw; zU`2=yEu(Gc%6qhXwWXh|2IbWt$;5gcwsI$|1}j2zXjv4@n|QCa=+5<%)u5s~NHQT> zHv`{6C#(i5LUd@^{`*Yj?qgZvgw>$3WRPS+bRjGgov<2ICJuzgp=DX=&&w^tvfT-* zL1p_O$%N=aSPeR1HK>Xj2#rGv&o{jWgHY85;r)X|KQ;nh>15cA(7nW;52ugYuo~3q zGf3c-6S7eVL{}%=Kw#zR-N0(lZS<$86FOfrAsgk~MV73PB|dLxAw#ELgOQ0VO0efs zM7MpA zgvOzT=XL_3bNcV=SLO!QIcs_4#1VCB{Kuyr5A4tAN4 zjdJd?50bzR>U&tPh|<;A2Pw&fY_xrl1TwKh-Pp=;vz$|si4v>a2T7o!^qUQ=11H0r zl1!}E!9GYrwmndlK`aR?I6@t%zmJrE!U9{>=|=$nwew`J0KbSKY!ZLUd?Z6nv&3x+{62_d!ZB zA-WKFo2UrUp=G<-uWzGdiQCZoASIa)J!GO2>VqUS4lT<{w^6zpI?fIynGoIfK@u8= z7M@$3vky{|iJmSegRi4M8Ny%wMalx}>+1JGN?^l@J_<(QmZ1|a;;!!#BUbvWCx1Si zqVAR6CMd~-Y!rfhkOX#6{ljgveUOq&$VS@-Ngxx|2mAH)UCn>e+Z`pDD6!h54-Rvo zqN?AkRyX9i_19sAnOLs_r%y$Q4lTP4_)J4|=Y0*%DanNBVF#Vi4wBF~wCq;XuWzGd zi4*FBlw?A5+XqQ#99niea~q|rq2p{(k_pjmA0(l1XyJM4gOp^Vr&AvsA=2*$<4st7 z5G$X;h7)}h$_W?I)lEO*Uj3aV_Q?qLK}rxS{q=9`rV+R;>5V1wr_w@(8i7pIdz2IE dADkCCW(4~n32LFzoT&O>zw-KTgi$L-{0E1Ezv%z~ literal 0 HcmV?d00001 diff --git a/models_NEEDTOMOVE/empty/meshes/Brick_1.STL b/models_NEEDTOMOVE/empty/meshes/Brick_1.STL new file mode 100755 index 0000000000000000000000000000000000000000..cfc1a6b33819e30a8fc74bf637743e1eed0ab9cf GIT binary patch literal 4884 zcmb`Kv2GMW42JibG#o|Xh76RAfY{711%+pih?F3 z5(N}Q!oO#{_pNU?n}Rg4#{T@z*q+&>Y5woenWtOt^y~8Y!^x``AGhy*UA3QX-Ce@o zR=d->Vh;%SOb}ZU@v~l8)N({adRp%_4v4G~JM4+|O3dZ)eVrpTwD+*7;k!B@lxvFc zllQyYS>A0u2MO6)_e>G3-|HZ6N zJG4w)&D0GEdPscx{KU_9a8Qc zJFKU3&_e?D%?Qm{)!rn69_F?4IT0_MW~5 z7Q0Ov^f0g4TlGmV-*8un=%di5q(RRx@BUR9YLMXDh2HEn{5w_i`fF)B)UGxA%R`(r``L( zike=`l6P&qx~w}1*lAZ2L2 z^qd~%MQ5iq;tQbu$M<|@5}DWTyicfZ@UT{xAKmjl3hQaE=pg|+Rd<5WlvKSs?LN0( z-#%K=!@PC9?e9k>+(V+;wq9PJ`j`IsgVT{HtL~zmB|H~s5~aGas~w|org=gdgP!B$ zMPsEtTCH}>J2F?6slL>ia1V)UySB2IzZbs$zIZ(n)v2Phgy#ZHqD)EbYQ-p=X`YbA zV3ts;q7nUYt$6!zA3e&}EmNLE)ge42s_oidE7$mQ(VmV(b?W9U;kiJQC_RsJ>=-rj z#Zjhl!aXFaZR>eTn&z+mKkMW%z4(S(USFRk+(V+;wqAC%pR185(_3`2gy#ZH!n@gc zul1HNs&>;fHuHouJXcIi(>(0|7geSv`l0u#eqT45*J}Gz%C<~-t}5Xk64mat%X@X_ z>AlmDs7@7~B|H~s5@|{pg)_|)($ISqRS4VG^IS2?-p8obJmI-WRNJ-Hj{RJXM0M)s bEaAC8lSp&LD4c1YkcQr?s8!gut||EkzrE#; literal 0 HcmV?d00001 diff --git a/models_NEEDTOMOVE/empty/model.config b/models_NEEDTOMOVE/empty/model.config new file mode 100755 index 0000000..9e574ea --- /dev/null +++ b/models_NEEDTOMOVE/empty/model.config @@ -0,0 +1,19 @@ + + + + Brick + 1.0 + model-1_2.sdf + model-1_3.sdf + model-1_4.sdf + model.sdf + + + RON + r.saputra@imperial.ac.uk + + + + Brick model + + diff --git a/models_NEEDTOMOVE/empty/model.sdf b/models_NEEDTOMOVE/empty/model.sdf new file mode 100755 index 0000000..83d6156 --- /dev/null +++ b/models_NEEDTOMOVE/empty/model.sdf @@ -0,0 +1,24 @@ + + + + true + + 0 0 0 0 -0 0 + + 0 0 0 1.5708 -0 0 + + + 1 1 1 + model://Brick/meshes/Brick_1.STL + + + + 0 1 0 1 + 0 1 0 1 + 0.1 0.1 0.1 1 + 0 0 0 0 + + + + +