Skip to content
Snippets Groups Projects
Commit 420ab135 authored by Christoph Schröter's avatar Christoph Schröter
Browse files

No commit message 

No commit message
parent 7d79e31a
Branches
Tags
No related merge requests found
Show whitespace changes
Inline Side-by-side
cobot1.xml
+ 333
− 146
View file @ 420ab135
...@@ -5,10 +5,16 @@ ...@@ -5,10 +5,16 @@
int retry_count = 0; int retry_count = 0;
int init_retry_count = 0; int init_retry_count = 0;
int place_glass_retry = 0; int place_glass_retry = 0;
int orient_constraint_set = 0;
chan pressed; chan pressed;
chan pickup_fail, pickup_success; chan pickup_fail, pickup_success;
chan place_fail, place_success;
chan move_fail, move_success; chan move_fail, move_success;
chan place_fail, place_success;</declaration> chan move_goal;
chan position_update;
int bottle_in_start_pos = 1;
int glass_in_start_pos = 1;
int glass_in_target_pos = 0;</declaration>
<template> <template>
<name>place</name> <name>place</name>
<location id="id0" x="0" y="0"> <location id="id0" x="0" y="0">
...@@ -30,33 +36,126 @@ chan place_fail, place_success;</declaration> ...@@ -30,33 +36,126 @@ chan place_fail, place_success;</declaration>
</transition> </transition>
</template> </template>
<template> <template>
<name>press</name> <name>model_positions</name>
<location id="id1" x="-3153" y="-3026"> <location id="id1" x="-1547" y="-450">
<name x="-1598" y="-479">start_pos</name>
</location>
<location id="id2" x="-1096" y="-285">
<name x="-1130" y="-331">bottle_moving</name>
</location>
<location id="id3" x="-1504" y="-242">
<name x="-1572" y="-280">bottle_end_pos</name>
</location>
<location id="id4" x="-1538" y="-85">
<name x="-1649" y="-76">glass_moving</name>
</location>
<location id="id5" x="-994" y="-221">
<name x="-1045" y="-246">bottle_and_glass_end_pos</name>
</location>
<location id="id6" x="-748" y="51">
</location> </location>
<location id="id2" x="-3017" y="-2983"> <location id="id7" x="-697" y="-127">
<name x="-680" y="-144">glass_back_to_start</name>
</location> </location>
<init ref="id1"/> <init ref="id1"/>
<transition>
<source ref="id4"/>
<target ref="id6"/>
<label kind="synchronisation" x="-1011" y="-76">position_update?</label>
<label kind="assignment" x="-1037" y="-59">glass_in_start_pos = 1</label>
<nail x="-1020" y="-68"/>
</transition>
<transition>
<source ref="id5"/>
<target ref="id7"/>
<label kind="synchronisation" x="-858" y="-195">position_update?</label>
<label kind="assignment" x="-892" y="-178">glass_in_target_pos = 0</label>
</transition>
<transition>
<source ref="id3"/>
<target ref="id2"/>
<label kind="synchronisation" x="-1487" y="-340">position_update?</label>
<label kind="assignment" x="-1479" y="-323">bottle_in_start_pos = 0</label>
<nail x="-1386" y="-330"/>
</transition>
<transition>
<source ref="id6"/>
<target ref="id4"/>
<label kind="synchronisation" x="-1147" y="34">position_update?</label>
<label kind="assignment" x="-1164" y="51">glass_in_start_pos = 0</label>
</transition>
<transition>
<source ref="id7"/>
<target ref="id6"/>
<label kind="synchronisation" x="-697" y="-42">position_update?</label>
<label kind="assignment" x="-705" y="-25">glass_in_start_pos = 1</label>
</transition>
<transition>
<source ref="id4"/>
<target ref="id5"/>
<label kind="synchronisation" x="-1241" y="-161">position_update?</label>
<label kind="assignment" x="-1266" y="-144">glass_in_target_pos = 1</label>
</transition>
<transition>
<source ref="id3"/>
<target ref="id4"/>
<label kind="synchronisation" x="-1674" y="-195">position_update?</label>
<label kind="assignment" x="-1700" y="-170">glass_in_start_pos = 0</label>
</transition>
<transition>
<source ref="id2"/>
<target ref="id3"/>
<label kind="synchronisation" x="-1249" y="-272">position_update?</label>
<label kind="assignment" x="-1292" y="-255">bottle_in_start_pos = 1</label>
</transition>
<transition> <transition>
<source ref="id1"/> <source ref="id1"/>
<target ref="id2"/> <target ref="id2"/>
<label kind="synchronisation" x="-1470" y="-493">position_update?</label>
<label kind="assignment" x="-1487" y="-467">bottle_in_start_pos = 0</label>
</transition>
</template>
<template>
<name>movegoal</name>
<location id="id8" x="0" y="0">
</location>
<init ref="id8"/>
<transition>
<source ref="id8"/>
<target ref="id8"/>
<label kind="synchronisation" x="-136" y="-102">move_goal!</label>
<nail x="-178" y="-187"/>
<nail x="178" y="-136"/>
</transition>
</template>
<template>
<name>press</name>
<location id="id9" x="-3153" y="-3026">
</location>
<location id="id10" x="-3017" y="-2983">
</location>
<init ref="id9"/>
<transition>
<source ref="id9"/>
<target ref="id10"/>
<label kind="synchronisation" x="-3119" y="-3034">pressed!</label> <label kind="synchronisation" x="-3119" y="-3034">pressed!</label>
</transition> </transition>
</template> </template>
<template> <template>
<name>move</name> <name>move</name>
<location id="id3" x="-612" y="-68"> <location id="id11" x="-612" y="-68">
</location> </location>
<init ref="id3"/> <init ref="id11"/>
<transition> <transition>
<source ref="id3"/> <source ref="id11"/>
<target ref="id3"/> <target ref="id11"/>
<label kind="synchronisation" x="-646" y="51">move_fail!</label> <label kind="synchronisation" x="-646" y="51">move_fail!</label>
<nail x="-663" y="42"/> <nail x="-663" y="42"/>
<nail x="-535" y="59"/> <nail x="-535" y="59"/>
</transition> </transition>
<transition> <transition>
<source ref="id3"/> <source ref="id11"/>
<target ref="id3"/> <target ref="id11"/>
<label kind="synchronisation" x="-653" y="-237">move_success!</label> <label kind="synchronisation" x="-653" y="-237">move_success!</label>
<nail x="-671" y="-229"/> <nail x="-671" y="-229"/>
<nail x="-501" y="-212"/> <nail x="-501" y="-212"/>
...@@ -64,19 +163,19 @@ chan place_fail, place_success;</declaration> ...@@ -64,19 +163,19 @@ chan place_fail, place_success;</declaration>
</template> </template>
<template> <template>
<name>pickup</name> <name>pickup</name>
<location id="id4" x="0" y="0"> <location id="id12" x="0" y="0">
</location> </location>
<init ref="id4"/> <init ref="id12"/>
<transition> <transition>
<source ref="id4"/> <source ref="id12"/>
<target ref="id4"/> <target ref="id12"/>
<label kind="synchronisation" x="-34" y="102">pickup_fail!</label> <label kind="synchronisation" x="-34" y="102">pickup_fail!</label>
<nail x="-76" y="93"/> <nail x="-76" y="93"/>
<nail x="110" y="76"/> <nail x="110" y="76"/>
</transition> </transition>
<transition> <transition>
<source ref="id4"/> <source ref="id12"/>
<target ref="id4"/> <target ref="id12"/>
<label kind="synchronisation" x="-109" y="-93">pickup_success!</label> <label kind="synchronisation" x="-109" y="-93">pickup_success!</label>
<nail x="-127" y="-153"/> <nail x="-127" y="-153"/>
<nail x="136" y="-102"/> <nail x="136" y="-102"/>
...@@ -85,88 +184,138 @@ chan place_fail, place_success;</declaration> ...@@ -85,88 +184,138 @@ chan place_fail, place_success;</declaration>
<template> <template>
<name x="5" y="5">Cobot</name> <name x="5" y="5">Cobot</name>
<declaration>// Place local declarations here.</declaration> <declaration>// Place local declarations here.</declaration>
<location id="id5" x="-102" y="42"> <location id="id13" x="-102" y="42">
<name x="-112" y="8">waiting</name> <name x="-112" y="8">waiting</name>
</location> </location>
<location id="id6" x="204" y="68"> <location id="id14" x="204" y="68">
<name x="229" y="76">pick_bottle</name> <name x="221" y="68">pick_bottle</name>
<label kind="invariant" x="127" y="85">orient_constraint_set == false</label>
</location> </location>
<location id="id7" x="595" y="42"> <location id="id15" x="595" y="42">
<name x="544" y="59">move_to_glass</name> <name x="578" y="25">move_to_glass</name>
</location> </location>
<location id="id8" x="1020" y="59"> <location id="id16" x="1020" y="59">
<name x="926" y="68">start_pour</name> <name x="926" y="68">start_pour</name>
</location> </location>
<location id="id9" x="442" y="-119"> <location id="id17" x="442" y="-119">
<name x="399" y="-153">shut_down</name> <name x="399" y="-153">shut_down</name>
</location> </location>
<location id="id10" x="918" y="289"> <location id="id18" x="918" y="289">
<name x="867" y="314">stop_pour</name> <name x="867" y="314">stop_pour</name>
</location> </location>
<location id="id11" x="442" y="289"> <location id="id19" x="442" y="289">
<name x="348" y="263">place_bottle</name> <name x="348" y="263">place_bottle</name>
</location> </location>
<location id="id12" x="17" y="357"> <location id="id20" x="-102" y="399">
<name x="17" y="365">pick_glass</name> <name x="-102" y="407">pick_glass</name>
<label kind="invariant" x="-119" y="382">bottle_in_start_pos == true</label>
</location> </location>
<location id="id13" x="833" y="493"> <location id="id21" x="833" y="493">
<name x="790" y="501">place_glass_target</name> <name x="790" y="501">place_glass_target</name>
</location> </location>
<location id="id14" x="850" y="637"> <location id="id22" x="850" y="637">
<name x="799" y="654">place_glass_orig</name> <name x="799" y="654">place_glass_orig</name>
</location> </location>
<location id="id15" x="1105" y="561"> <location id="id23" x="1343" y="595">
<name x="1096" y="527">goto_last_state</name> <name x="1343" y="612">goto_last_state</name>
</location> </location>
<location id="id16" x="663" y="586"> <location id="id24" x="663" y="586">
<name x="501" y="569">goto_last_state_orig</name> <name x="501" y="569">goto_last_state_orig</name>
</location> </location>
<location id="id17" x="1156" y="391"> <location id="id25" x="1275" y="459">
<name x="1105" y="365">move_to_start_pos</name> <name x="1258" y="476">move_to_start_pos</name>
</location>
<location id="id26" x="1428" y="348">
<name x="1418" y="314">finished_success</name>
</location>
<location id="id27" x="340" y="8">
</location>
<location id="id28" x="858" y="42">
</location> </location>
<location id="id18" x="1317" y="425"> <location id="id29" x="391" y="535">
<name x="1307" y="391">finished_success</name>
</location> </location>
<init ref="id5"/> <location id="id30" x="1224" y="357">
<label kind="invariant" x="1173" y="348">glass_in_target_pos == 1</label>
</location>
<location id="id31" x="102" y="612">
</location>
<location id="id32" x="1224" y="544">
<name x="1156" y="544">stepping_back</name>
</location>
<init ref="id13"/>
<transition> <transition>
<source ref="id17"/> <source ref="id32"/>
<target ref="id18"/> <target ref="id23"/>
<label kind="synchronisation" x="1190" y="391">move_success?</label> <label kind="synchronisation" x="1249" y="561">move_success?</label>
</transition> </transition>
<transition> <transition>
<source ref="id13"/> <source ref="id31"/>
<target ref="id17"/> <target ref="id20"/>
<label kind="synchronisation" x="1028" y="408">place_success?</label> <label kind="synchronisation" x="-17" y="518">move_goal?</label>
<label kind="assignment" x="-17" y="535">orient_constraint_set = false</label>
</transition> </transition>
<transition> <transition>
<source ref="id16"/> <source ref="id30"/>
<target ref="id9"/> <target ref="id25"/>
<nail x="714" y="-93"/> <label kind="synchronisation" x="1224" y="399">move_goal?</label>
<label kind="assignment" x="1198" y="416">orient_constraint_set = false</label>
</transition> </transition>
<transition> <transition>
<source ref="id14"/> <source ref="id29"/>
<target ref="id21"/>
<label kind="synchronisation" x="493" y="493">move_goal?</label>
<label kind="assignment" x="425" y="510">orient_constraint_set = true</label>
</transition>
<transition>
<source ref="id28"/>
<target ref="id16"/> <target ref="id16"/>
<label kind="guard" x="681" y="577">retry_count == 1</label> <label kind="synchronisation" x="875" y="25">move_goal?</label>
<label kind="synchronisation" x="681" y="594">place_fail?</label> <label kind="assignment" x="867" y="42">orient_constraint_set = false</label>
</transition> </transition>
<transition> <transition>
<source ref="id12"/> <source ref="id27"/>
<target ref="id9"/> <target ref="id15"/>
<label kind="synchronisation" x="442" y="0">move_goal?</label>
<label kind="assignment" x="382" y="17">orient_constraint_set = true</label>
</transition>
<transition>
<source ref="id25"/>
<target ref="id26"/>
<label kind="synchronisation" x="1334" y="382">move_success?</label>
</transition>
<transition>
<source ref="id21"/>
<target ref="id30"/>
<label kind="synchronisation" x="1054" y="399">place_success?</label>
</transition>
<transition>
<source ref="id24"/>
<target ref="id17"/>
<nail x="714" y="-93"/>
</transition>
<transition>
<source ref="id22"/>
<target ref="id24"/>
<label kind="guard" x="671" y="578">retry_count == 1</label>
<label kind="synchronisation" x="697" y="595">place_fail?</label>
</transition>
<transition>
<source ref="id20"/>
<target ref="id17"/>
<label kind="guard" x="459" y="416">place_glass_retry == 0</label> <label kind="guard" x="459" y="416">place_glass_retry == 0</label>
<label kind="synchronisation" x="467" y="433">pickup_success?</label> <label kind="synchronisation" x="467" y="433">pickup_success?</label>
<nail x="603" y="467"/> <nail x="603" y="467"/>
<nail x="663" y="-59"/> <nail x="663" y="-59"/>
</transition> </transition>
<transition> <transition>
<source ref="id14"/> <source ref="id22"/>
<target ref="id12"/> <target ref="id31"/>
<label kind="synchronisation" x="255" y="620">place_success?</label> <label kind="synchronisation" x="255" y="629">place_success?</label>
<label kind="assignment" x="255" y="637">place_glass_retry--</label> <label kind="assignment" x="255" y="646">place_glass_retry--</label>
<nail x="238" y="620"/>
</transition> </transition>
<transition> <transition>
<source ref="id14"/> <source ref="id22"/>
<target ref="id14"/> <target ref="id22"/>
<label kind="guard" x="808" y="556">retry_count &gt; 1</label> <label kind="guard" x="808" y="556">retry_count &gt; 1</label>
<label kind="synchronisation" x="808" y="573">place_fail?</label> <label kind="synchronisation" x="808" y="573">place_fail?</label>
<label kind="assignment" x="808" y="590">retry_count--</label> <label kind="assignment" x="808" y="590">retry_count--</label>
...@@ -174,34 +323,34 @@ chan place_fail, place_success;</declaration> ...@@ -174,34 +323,34 @@ chan place_fail, place_success;</declaration>
<nail x="867" y="586"/> <nail x="867" y="586"/>
</transition> </transition>
<transition> <transition>
<source ref="id15"/> <source ref="id23"/>
<target ref="id14"/> <target ref="id22"/>
<label kind="synchronisation" x="960" y="569">move_success?</label> <label kind="synchronisation" x="1122" y="612">move_success?</label>
<label kind="assignment" x="918" y="586">retry_count = init_retry_count</label> <label kind="assignment" x="1096" y="629">retry_count = init_retry_count</label>
</transition> </transition>
<transition> <transition>
<source ref="id14"/> <source ref="id22"/>
<target ref="id14"/> <target ref="id22"/>
<label kind="synchronisation" x="979" y="633">move_fail?</label> <label kind="synchronisation" x="979" y="633">move_fail?</label>
<nail x="1071" y="629"/> <nail x="1071" y="629"/>
<nail x="1080" y="663"/> <nail x="1080" y="663"/>
</transition> </transition>
<transition> <transition>
<source ref="id14"/> <source ref="id22"/>
<target ref="id14"/> <target ref="id22"/>
<label kind="synchronisation" x="909" y="680">move_success?</label> <label kind="synchronisation" x="909" y="680">move_success?</label>
<nail x="926" y="671"/> <nail x="926" y="671"/>
<nail x="901" y="697"/> <nail x="901" y="697"/>
</transition> </transition>
<transition> <transition>
<source ref="id13"/> <source ref="id21"/>
<target ref="id15"/> <target ref="id32"/>
<label kind="guard" x="960" y="518">retry_count == 1</label> <label kind="guard" x="960" y="518">retry_count == 1</label>
<label kind="synchronisation" x="969" y="535">place_fail?</label> <label kind="synchronisation" x="969" y="535">place_fail?</label>
</transition> </transition>
<transition> <transition>
<source ref="id13"/> <source ref="id21"/>
<target ref="id13"/> <target ref="id21"/>
<label kind="guard" x="909" y="433">retry_count &gt; 1</label> <label kind="guard" x="909" y="433">retry_count &gt; 1</label>
<label kind="synchronisation" x="926" y="450">place_fail?</label> <label kind="synchronisation" x="926" y="450">place_fail?</label>
<label kind="assignment" x="935" y="467">retry_count--</label> <label kind="assignment" x="935" y="467">retry_count--</label>
...@@ -209,73 +358,72 @@ chan place_fail, place_success;</declaration> ...@@ -209,73 +358,72 @@ chan place_fail, place_success;</declaration>
<nail x="994" y="510"/> <nail x="994" y="510"/>
</transition> </transition>
<transition> <transition>
<source ref="id13"/> <source ref="id21"/>
<target ref="id13"/> <target ref="id21"/>
<label kind="synchronisation" x="850" y="408">move_fail?</label> <label kind="synchronisation" x="850" y="408">move_fail?</label>
<nail x="833" y="416"/> <nail x="833" y="416"/>
<nail x="892" y="433"/> <nail x="892" y="433"/>
</transition> </transition>
<transition> <transition>
<source ref="id13"/> <source ref="id21"/>
<target ref="id13"/> <target ref="id21"/>
<label kind="synchronisation" x="723" y="442">move_success?</label> <label kind="synchronisation" x="723" y="442">move_success?</label>
<nail x="705" y="425"/> <nail x="705" y="425"/>
<nail x="799" y="425"/> <nail x="799" y="425"/>
</transition> </transition>
<transition> <transition>
<source ref="id12"/> <source ref="id20"/>
<target ref="id13"/> <target ref="id29"/>
<label kind="guard" x="263" y="476">place_glass_retry &gt; 0</label> <label kind="guard" x="212" y="476">place_glass_retry &gt; 0</label>
<label kind="synchronisation" x="263" y="459">pickup_success?</label> <label kind="synchronisation" x="212" y="459">pickup_success?</label>
<label kind="assignment" x="263" y="493">retry_count = init_retry_count</label> <label kind="assignment" x="195" y="493">retry_count = init_retry_count</label>
<nail x="493" y="510"/>
</transition> </transition>
<transition> <transition>
<source ref="id12"/> <source ref="id20"/>
<target ref="id9"/> <target ref="id17"/>
<label kind="guard" x="204" y="221">retry_count == 1</label> <label kind="guard" x="204" y="221">retry_count == 1</label>
<label kind="synchronisation" x="221" y="238">pickup_fail?</label> <label kind="synchronisation" x="221" y="238">pickup_fail?</label>
<nail x="348" y="221"/> <nail x="348" y="221"/>
</transition> </transition>
<transition> <transition>
<source ref="id12"/> <source ref="id20"/>
<target ref="id12"/> <target ref="id20"/>
<label kind="guard" x="-85" y="433">retry_count &gt; 1</label> <label kind="guard" x="-204" y="475">retry_count &gt; 1</label>
<label kind="synchronisation" x="-85" y="450">pickup_fail?</label> <label kind="synchronisation" x="-204" y="492">pickup_fail?</label>
<label kind="assignment" x="-85" y="467">retry_count--</label> <label kind="assignment" x="-204" y="509">retry_count--</label>
<nail x="-103" y="518"/> <nail x="-222" y="560"/>
<nail x="-10" y="543"/> <nail x="-129" y="585"/>
</transition> </transition>
<transition> <transition>
<source ref="id12"/> <source ref="id20"/>
<target ref="id12"/> <target ref="id20"/>
<label kind="synchronisation" x="-102" y="340">move_fail?</label> <label kind="synchronisation" x="-221" y="382">move_fail?</label>
<nail x="-68" y="340"/> <nail x="-187" y="382"/>
<nail x="-76" y="357"/> <nail x="-195" y="399"/>
</transition> </transition>
<transition> <transition>
<source ref="id12"/> <source ref="id20"/>
<target ref="id12"/> <target ref="id20"/>
<label kind="synchronisation" x="1" y="293">move_success?</label> <label kind="synchronisation" x="-118" y="335">move_success?</label>
<nail x="51" y="306"/> <nail x="-68" y="348"/>
<nail x="-17" y="314"/> <nail x="-136" y="356"/>
</transition> </transition>
<transition> <transition>
<source ref="id11"/> <source ref="id19"/>
<target ref="id12"/> <target ref="id20"/>
<label kind="synchronisation" x="212" y="306">place_success?</label> <label kind="synchronisation" x="255" y="297">place_success?</label>
<label kind="assignment" x="51" y="323">retry_count = init_retry_count, place_glass_retry = init_retry_count</label> <label kind="assignment" x="119" y="323">retry_count = init_retry_count, place_glass_retry = init_retry_count</label>
</transition> </transition>
<transition> <transition>
<source ref="id11"/> <source ref="id19"/>
<target ref="id9"/> <target ref="id17"/>
<label kind="guard" x="374" y="110">retry_count == 1</label> <label kind="guard" x="374" y="110">retry_count == 1</label>
<label kind="synchronisation" x="391" y="93">place_fail?</label> <label kind="synchronisation" x="391" y="93">place_fail?</label>
<nail x="382" y="161"/> <nail x="382" y="161"/>
</transition> </transition>
<transition> <transition>
<source ref="id11"/> <source ref="id19"/>
<target ref="id11"/> <target ref="id19"/>
<label kind="guard" x="450" y="161">retry_count &gt; 1</label> <label kind="guard" x="450" y="161">retry_count &gt; 1</label>
<label kind="synchronisation" x="434" y="182">place_fail?</label> <label kind="synchronisation" x="434" y="182">place_fail?</label>
<label kind="assignment" x="434" y="199">retry_count--</label> <label kind="assignment" x="434" y="199">retry_count--</label>
...@@ -283,50 +431,50 @@ chan place_fail, place_success;</declaration> ...@@ -283,50 +431,50 @@ chan place_fail, place_success;</declaration>
<nail x="552" y="204"/> <nail x="552" y="204"/>
</transition> </transition>
<transition> <transition>
<source ref="id11"/> <source ref="id19"/>
<target ref="id11"/> <target ref="id19"/>
<label kind="synchronisation" x="460" y="306">move_success?</label> <label kind="synchronisation" x="476" y="348">move_success?</label>
<nail x="459" y="374"/> <nail x="459" y="374"/>
<nail x="510" y="357"/> <nail x="510" y="357"/>
</transition> </transition>
<transition> <transition>
<source ref="id11"/> <source ref="id19"/>
<target ref="id11"/> <target ref="id19"/>
<label kind="synchronisation" x="374" y="382">move_fail?</label> <label kind="synchronisation" x="374" y="382">move_fail?</label>
<nail x="365" y="399"/> <nail x="365" y="399"/>
<nail x="433" y="408"/> <nail x="433" y="408"/>
<nail x="442" y="306"/> <nail x="442" y="306"/>
</transition> </transition>
<transition> <transition>
<source ref="id6"/> <source ref="id14"/>
<target ref="id6"/> <target ref="id14"/>
<label kind="synchronisation" x="272" y="136">move_fail?</label> <label kind="synchronisation" x="272" y="136">move_fail?</label>
<nail x="229" y="153"/> <nail x="229" y="153"/>
<nail x="280" y="161"/> <nail x="280" y="161"/>
</transition> </transition>
<transition> <transition>
<source ref="id6"/> <source ref="id14"/>
<target ref="id6"/> <target ref="id14"/>
<label kind="synchronisation" x="51" y="85">move_success?</label> <label kind="synchronisation" x="51" y="102">move_success?</label>
<nail x="119" y="119"/> <nail x="119" y="119"/>
<nail x="178" y="127"/> <nail x="178" y="127"/>
</transition> </transition>
<transition> <transition>
<source ref="id10"/> <source ref="id18"/>
<target ref="id11"/> <target ref="id19"/>
<label kind="synchronisation" x="595" y="263">move_success?</label> <label kind="synchronisation" x="595" y="263">move_success?</label>
<label kind="assignment" x="603" y="289">retry_count = init_retry_count</label> <label kind="assignment" x="603" y="289">retry_count = init_retry_count</label>
</transition> </transition>
<transition> <transition>
<source ref="id10"/> <source ref="id18"/>
<target ref="id9"/> <target ref="id17"/>
<label kind="guard" x="1181" y="0">retry_count == 1</label> <label kind="guard" x="1181" y="0">retry_count == 1</label>
<label kind="synchronisation" x="1232" y="-25">pickup_fail?</label> <label kind="synchronisation" x="1232" y="-25">pickup_fail?</label>
<nail x="1436" y="-93"/> <nail x="1436" y="-93"/>
</transition> </transition>
<transition> <transition>
<source ref="id10"/> <source ref="id18"/>
<target ref="id10"/> <target ref="id18"/>
<label kind="guard" x="1071" y="323">retry_count &gt; 1</label> <label kind="guard" x="1071" y="323">retry_count &gt; 1</label>
<label kind="synchronisation" x="1079" y="306">move_fail?</label> <label kind="synchronisation" x="1079" y="306">move_fail?</label>
<label kind="assignment" x="1062" y="340">retry_count--</label> <label kind="assignment" x="1062" y="340">retry_count--</label>
...@@ -334,44 +482,44 @@ chan place_fail, place_success;</declaration> ...@@ -334,44 +482,44 @@ chan place_fail, place_success;</declaration>
<nail x="1037" y="382"/> <nail x="1037" y="382"/>
</transition> </transition>
<transition> <transition>
<source ref="id8"/> <source ref="id16"/>
<target ref="id10"/> <target ref="id18"/>
<label kind="synchronisation" x="884" y="102">move_success?</label> <label kind="synchronisation" x="884" y="102">move_success?</label>
<label kind="assignment" x="765" y="119">retry_count = init_retry_count</label> <label kind="assignment" x="765" y="119">retry_count = init_retry_count</label>
</transition> </transition>
<transition> <transition>
<source ref="id8"/> <source ref="id16"/>
<target ref="id8"/> <target ref="id16"/>
<label kind="synchronisation" x="969" y="-42">move_fail?</label> <label kind="synchronisation" x="969" y="-42">move_fail?</label>
<nail x="918" y="-25"/> <nail x="918" y="-25"/>
<nail x="1139" y="-25"/> <nail x="1139" y="-25"/>
</transition> </transition>
<transition> <transition>
<source ref="id7"/> <source ref="id15"/>
<target ref="id8"/> <target ref="id28"/>
<label kind="synchronisation" x="714" y="25">move_success?</label> <label kind="synchronisation" x="697" y="25">move_success?</label>
</transition> </transition>
<transition> <transition>
<source ref="id7"/> <source ref="id15"/>
<target ref="id7"/> <target ref="id15"/>
<label kind="synchronisation" x="544" y="-51">move_fail?</label> <label kind="synchronisation" x="544" y="-42">move_fail?</label>
<nail x="518" y="-8"/> <nail x="518" y="-8"/>
<nail x="637" y="-25"/> <nail x="637" y="-25"/>
</transition> </transition>
<transition> <transition>
<source ref="id6"/> <source ref="id14"/>
<target ref="id9"/> <target ref="id17"/>
<label kind="guard" x="280" y="-102">retry_count == 1</label> <label kind="guard" x="280" y="-102">retry_count == 1</label>
<label kind="synchronisation" x="306" y="-119">pickup_fail?</label> <label kind="synchronisation" x="306" y="-119">pickup_fail?</label>
</transition> </transition>
<transition> <transition>
<source ref="id6"/> <source ref="id14"/>
<target ref="id7"/> <target ref="id27"/>
<label kind="synchronisation" x="365" y="25">pickup_success?</label> <label kind="synchronisation" x="246" y="34">pickup_success?</label>
</transition> </transition>
<transition> <transition>
<source ref="id6"/> <source ref="id14"/>
<target ref="id6"/> <target ref="id14"/>
<label kind="guard" x="110" y="-102">retry_count &gt; 1</label> <label kind="guard" x="110" y="-102">retry_count &gt; 1</label>
<label kind="synchronisation" x="110" y="-85">pickup_fail?</label> <label kind="synchronisation" x="110" y="-85">pickup_fail?</label>
<label kind="assignment" x="110" y="-68">retry_count--</label> <label kind="assignment" x="110" y="-68">retry_count--</label>
...@@ -379,16 +527,55 @@ chan place_fail, place_success;</declaration> ...@@ -379,16 +527,55 @@ chan place_fail, place_success;</declaration>
<nail x="289" y="-42"/> <nail x="289" y="-42"/>
</transition> </transition>
<transition> <transition>
<source ref="id5"/> <source ref="id13"/>
<target ref="id6"/> <target ref="id14"/>
<label kind="select" x="-102" y="144">i : int[1,5]</label> <label kind="select" x="-272" y="34">i : int[1,5]</label>
<label kind="synchronisation" x="-102" y="161">pressed?</label> <label kind="synchronisation" x="-272" y="51">pressed?</label>
<label kind="assignment" x="-102" y="178">init_retry_count = i, retry_count = init_retry_count</label> <label kind="assignment" x="-272" y="68">init_retry_count = i, retry_count = init_retry_count</label>
</transition>
</template>
<template>
<name>positionupdatecatcher</name>
<location id="id33" x="-586" y="-170">
</location>
<init ref="id33"/>
<transition>
<source ref="id33"/>
<target ref="id33"/>
<label kind="synchronisation" x="-314" y="-195">position_update?</label>
<nail x="-416" y="-272"/>
<nail x="-391" y="-127"/>
</transition>
</template>
<template>
<name>positionupdate</name>
<location id="id34" x="0" y="0">
</location>
<init ref="id34"/>
<transition>
<source ref="id34"/>
<target ref="id34"/>
<label kind="synchronisation" x="34" y="-76">position_update!</label>
<nail x="-25" y="-119"/>
<nail x="178" y="-17"/>
</transition>
</template>
<template>
<name>movegoalcatcher</name>
<location id="id35" x="-603" y="-229">
</location>
<init ref="id35"/>
<transition>
<source ref="id35"/>
<target ref="id35"/>
<label kind="synchronisation" x="-450" y="-238">move_goal?</label>
<nail x="-442" y="-323"/>
<nail x="-450" y="-119"/>
</transition> </transition>
</template> </template>
<system>// Place template instantiations here. <system>// Place template instantiations here.
// List one or more processes to be composed into a system. // List one or more processes to be composed into a system.
system Cobot, press, move, pickup, place; system Cobot, press, move, pickup, place, movegoal, movegoalcatcher, positionupdate, positionupdatecatcher, model_positions;
</system> </system>
<queries> <queries>
<query> <query>
......
src/tron_adapter.cpp
+ 95
− 6
View file @ 420ab135
...@@ -10,8 +10,12 @@ ...@@ -10,8 +10,12 @@
#include <moveit_msgs/PickupAction.h> #include <moveit_msgs/PickupAction.h>
#include <moveit_msgs/PlaceAction.h> #include <moveit_msgs/PlaceAction.h>
#include <moveit_msgs/MoveItErrorCodes.h> #include <moveit_msgs/MoveItErrorCodes.h>
#include <moveit_msgs/MoveGroupActionResult.h> #include <moveit_msgs/MoveGroupAction.h>
#include <control_msgs/FollowJointTrajectoryAction.h> #include <control_msgs/FollowJointTrajectoryAction.h>
#include <gazebo_msgs/LinkStates.h>
#include <geometry_msgs/Pose.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Vector3.h>
const std::string IP = "127.0.0.1"; const std::string IP = "127.0.0.1";
const uint16_t PORT = 8080; const uint16_t PORT = 8080;
...@@ -395,8 +399,7 @@ void mappings_callback_to_TRON(const ros::MessageEvent<T>& event){ ...@@ -395,8 +399,7 @@ void mappings_callback_to_TRON(const ros::MessageEvent<T>& event){
} }
int32_t retry_count; int32_t retry_count; // set in configuration_phase
void on_pressure(const cooperation_msgs::PressureMsg::ConstPtr &msg) { void on_pressure(const cooperation_msgs::PressureMsg::ConstPtr &msg) {
if (msg->pressed) if (msg->pressed)
report_now("pressed", 1, &retry_count); report_now("pressed", 1, &retry_count);
...@@ -420,11 +423,87 @@ void on_move_result(const control_msgs::FollowJointTrajectoryActionResult::Const ...@@ -420,11 +423,87 @@ void on_move_result(const control_msgs::FollowJointTrajectoryActionResult::Const
else report_now("move_fail"); else report_now("move_fail");
} }
void on_move_goal(const moveit_msgs::MoveGroupActionGoal::ConstPtr &msg){
int32_t is_orientation_constraint_set;
if (msg->goal.request.path_constraints.orientation_constraints.empty())
is_orientation_constraint_set = 0;
else is_orientation_constraint_set = 1;
report_now("move_goal", 1, &is_orientation_constraint_set);
}
bool compare_poses(geometry_msgs::Pose p1, geometry_msgs::Pose p2, double pos_tolerance = 0.01, double angle_tol = 0.1){
// q1 is inverse orientation from p1 (see w)
tf2::Quaternion q1(p1.orientation.x, p1.orientation.y, p1.orientation.z, -p1.orientation.w);
tf2::Quaternion q2(p2.orientation.x, p2.orientation.y, p2.orientation.z, p2.orientation.w);
tf2::Quaternion q_diff = q2 * q1; // order is important with quaternions
tf2Scalar angle_diff = q_diff.getAngleShortestPath();
double x_d = p1.position.x - p2.position.x;
double y_d = p1.position.y - p2.position.y;
double z_d = p1.position.z - p2.position.z;
double pos_diff = std::sqrt(x_d*x_d + y_d*y_d + z_d*z_d);
if (angle_diff < angle_tol * M_PI && pos_diff < pos_tolerance)
return true;
return false;
}
const std::vector<std::string> tron_vars_ordered = {
"bottle_in_start_pos",
"glass_in_start_pos",
"glass_in_target_pos"
};
std::vector<std::string> model_names = { "bottle::link", "glass::link", "glass::link"};
std::vector<geometry_msgs::Pose> pose_to_compare_against; // set in configuration_phase
std::vector<bool> init_states = {1,1,0};
bool gazebo_initialized = false;
void on_gazebo_link_states(const gazebo_msgs::LinkStates::ConstPtr &msg){
int32_t var[tron_vars_ordered.size()];
for (int i = 0; i < tron_vars_ordered.size(); i++) {
for (int j = 0; j < msg->name.size(); j++) {
if (msg->name[j] == model_names[i]) {
if (compare_poses(msg->pose[j], pose_to_compare_against[i]))
var[i] = 1;
else var[i] = 0;
break;
}
}
}
if (gazebo_initialized)
report_now("position_update", static_cast<uint16_t>(tron_vars_ordered.size()), var);
else {
for (int i = 0; i < tron_vars_ordered.size(); i++)
if (var[i] != init_states[i]) return;
gazebo_initialized = true;
}
}
void configuration_phase(ros::NodeHandle& nh){ void configuration_phase(ros::NodeHandle& nh){
/* note: for configuration phase maximum message length is 256 Bytes, /* note: for configuration phase maximum message length is 256 Bytes,
therefore heap allocation can be avoided most of the time in called functions */ therefore heap allocation can be avoided most of the time in called functions */
nh.getParam("/planning/retries", retry_count); nh.getParam("/planning/retries", retry_count);
geometry_msgs::Pose pose;
nh.getParam("/object/bottle/x", pose.position.x);
nh.getParam("/object/bottle/y", pose.position.y);
nh.getParam("/object/bottle/z", pose.position.z);
pose.orientation.w = 1.0;
pose_to_compare_against.push_back(pose);
nh.getParam("/object/glass/x", pose.position.x);
nh.getParam("/object/glass/y", pose.position.y);
nh.getParam("/object/glass/z", pose.position.z);
pose.orientation.w = 1.0;
pose_to_compare_against.push_back(pose);
nh.getParam("/object/glass/place/x", pose.position.x);
nh.getParam("/object/glass/place/y", pose.position.y);
nh.getParam("/object/glass/place/z", pose.position.z);
pose.orientation.w = 1.0;
pose_to_compare_against.push_back(pose);
// note: since we are not an actual client (or server) // note: since we are not an actual client (or server)
// we need to use the high level packet::*Action* messages. // we need to use the high level packet::*Action* messages.
// custom callbacks are implemented in order to not worry about header size // custom callbacks are implemented in order to not worry about header size
...@@ -452,7 +531,17 @@ void configuration_phase(ros::NodeHandle& nh){ ...@@ -452,7 +531,17 @@ void configuration_phase(ros::NodeHandle& nh){
mappings.push_back(map); mappings.push_back(map);
output_subscribers.push_back(nh.subscribe("/position_joint_trajectory_controller/follow_joint_trajectory/result", 10, on_move_result)); output_subscribers.push_back(nh.subscribe("/position_joint_trajectory_controller/follow_joint_trajectory/result", 10, on_move_result));
// TODO: place and variables map = Mapping("/move_group/goal", "move_goal", false);
map.add_var_to_mapping("orient_constraint_set");
mappings.push_back(map);
output_subscribers.push_back(nh.subscribe("/move_group/goal", 10, on_move_goal));
map = Mapping("/gazebo/link_states", "position_update", false);
map.add_var_to_mapping("bottle_in_start_pos");
map.add_var_to_mapping("glass_in_start_pos");
map.add_var_to_mapping("glass_in_target_pos");
mappings.push_back(map);
output_subscribers.push_back(nh.subscribe("/gazebo/link_states", 1, on_gazebo_link_states));
// not obvious in documentation: local variables are not supported // not obvious in documentation: local variables are not supported
// add_var_to_channel(socketfd, "ausloesen", "lokal"); // add_var_to_channel(socketfd, "ausloesen", "lokal");
...@@ -542,7 +631,7 @@ int main(int argc, char**argv){ ...@@ -542,7 +631,7 @@ int main(int argc, char**argv){
request_start(); request_start();
// testing phase loop // testing phase loop
ros::Rate test_phase_freq(60); ros::Rate test_phase_freq(10);
while (ros::ok()) { while (ros::ok()) {
ros::spinOnce(); ros::spinOnce();
process_TRONs_msgs(); process_TRONs_msgs();
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment