diff --git a/cobot1.xml b/cobot1.xml
index 94002a9b30dcc0d9ba8edb44bd6672efad0a293a..6dd709d9f333623bc4ef0a7a721238b01e6960c8 100644
--- a/cobot1.xml
+++ b/cobot1.xml
@@ -10,7 +10,7 @@ chan pressed;
chan pickup_fail, pickup_success;
chan place_fail, place_success;
chan move_fail, move_success;
-chan move_goal;
+chan goal;
chan position_update;
int bottle_in_start_pos = 1;
int glass_in_start_pos = 1;
@@ -23,122 +23,122 @@ int glass_in_target_pos = 0;</declaration>
<transition>
<source ref="id0"/>
<target ref="id0"/>
- <label kind="synchronisation" x="-76" y="110">place_success!</label>
- <nail x="-86" y="102"/>
- <nail x="118" y="119"/>
+ <label kind="synchronisation" x="-42" y="51">place_success!</label>
+ <nail x="-34" y="51"/>
+ <nail x="34" y="51"/>
</transition>
<transition>
<source ref="id0"/>
<target ref="id0"/>
- <label kind="synchronisation" x="-67" y="-72">place_fail!</label>
- <nail x="-85" y="-110"/>
- <nail x="170" y="-68"/>
+ <label kind="synchronisation" x="-42" y="-68">place_fail!</label>
+ <nail x="-34" y="-51"/>
+ <nail x="34" y="-51"/>
</transition>
</template>
<template>
<name>model_positions</name>
- <location id="id1" x="-1547" y="-450">
- <name x="-1598" y="-479">start_pos</name>
+ <location id="id1" x="-1547" y="-416">
+ <name x="-1598" y="-445">start_pos</name>
</location>
- <location id="id2" x="-1096" y="-285">
- <name x="-1130" y="-331">bottle_moving</name>
+ <location id="id2" x="-1343" y="-416">
+ <name x="-1326" y="-425">bottle_moving</name>
</location>
- <location id="id3" x="-1504" y="-242">
- <name x="-1572" y="-280">bottle_end_pos</name>
+ <location id="id3" x="-1343" y="-297">
+ <name x="-1326" y="-323">bottle_end_pos</name>
</location>
- <location id="id4" x="-1538" y="-85">
- <name x="-1649" y="-76">glass_moving</name>
+ <location id="id4" x="-1343" y="-187">
+ <name x="-1454" y="-178">glass_moving</name>
</location>
- <location id="id5" x="-994" y="-221">
- <name x="-1045" y="-246">bottle_and_glass_end_pos</name>
+ <location id="id5" x="-1079" y="-187">
+ <name x="-1173" y="-170">bottle_and_glass_end_pos</name>
</location>
- <location id="id6" x="-748" y="51">
+ <location id="id6" x="-986" y="8">
</location>
- <location id="id7" x="-697" y="-127">
- <name x="-680" y="-144">glass_back_to_start</name>
+ <location id="id7" x="-986" y="-323">
+ <name x="-969" y="-340">glass_back_to_start</name>
</location>
<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"/>
+ <label kind="synchronisation" x="-1283" y="-85">position_update?</label>
+ <label kind="assignment" x="-1283" y="-68">glass_in_start_pos = 1</label>
</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>
+ <label kind="synchronisation" x="-1147" y="-289">position_update?</label>
+ <label kind="assignment" x="-1198" y="-272">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"/>
+ <label kind="synchronisation" x="-1572" y="-365">position_update?</label>
+ <label kind="assignment" x="-1572" y="-348">bottle_in_start_pos = 0</label>
+ <nail x="-1411" y="-348"/>
</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>
+ <label kind="synchronisation" x="-1470" y="-51">position_update?</label>
+ <label kind="assignment" x="-1504" y="-34">glass_in_start_pos = 0</label>
+ <nail x="-1343" y="8"/>
</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>
+ <label kind="synchronisation" x="-977" y="-272">position_update?</label>
+ <label kind="assignment" x="-977" y="-255">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>
+ <label kind="synchronisation" x="-1309" y="-221">position_update?</label>
+ <label kind="assignment" x="-1309" y="-204">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>
+ <label kind="synchronisation" x="-1504" y="-263">position_update?</label>
+ <label kind="assignment" x="-1504" y="-246">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>
+ <label kind="synchronisation" x="-1334" y="-391">position_update?</label>
+ <label kind="assignment" x="-1334" y="-374">bottle_in_start_pos = 1</label>
</transition>
<transition>
<source ref="id1"/>
<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>
+ <label kind="synchronisation" x="-1479" y="-459">position_update?</label>
+ <label kind="assignment" x="-1513" y="-442">bottle_in_start_pos = 0</label>
</transition>
</template>
<template>
- <name>movegoal</name>
+ <name>goal_</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"/>
+ <label kind="synchronisation" x="-42" y="-51">goal!</label>
+ <nail x="-25" y="-34"/>
+ <nail x="25" y="-34"/>
</transition>
</template>
<template>
<name>press</name>
<location id="id9" x="-3153" y="-3026">
</location>
- <location id="id10" x="-3017" y="-2983">
+ <location id="id10" x="-3026" y="-3026">
</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="-3043">pressed!</label>
</transition>
</template>
<template>
@@ -149,16 +149,16 @@ int glass_in_target_pos = 0;</declaration>
<transition>
<source ref="id11"/>
<target ref="id11"/>
- <label kind="synchronisation" x="-646" y="51">move_fail!</label>
- <nail x="-663" y="42"/>
- <nail x="-535" y="59"/>
+ <label kind="synchronisation" x="-646" y="-25">move_fail!</label>
+ <nail x="-629" y="-34"/>
+ <nail x="-595" y="-34"/>
</transition>
<transition>
<source ref="id11"/>
<target ref="id11"/>
- <label kind="synchronisation" x="-653" y="-237">move_success!</label>
- <nail x="-671" y="-229"/>
- <nail x="-501" y="-212"/>
+ <label kind="synchronisation" x="-663" y="-119">move_success!</label>
+ <nail x="-629" y="-102"/>
+ <nail x="-595" y="-102"/>
</transition>
</template>
<template>
@@ -169,414 +169,454 @@ int glass_in_target_pos = 0;</declaration>
<transition>
<source ref="id12"/>
<target ref="id12"/>
- <label kind="synchronisation" x="-34" y="102">pickup_fail!</label>
- <nail x="-76" y="93"/>
- <nail x="110" y="76"/>
+ <label kind="synchronisation" x="-34" y="34">pickup_fail!</label>
+ <nail x="-17" y="34"/>
+ <nail x="17" y="34"/>
</transition>
<transition>
<source ref="id12"/>
<target ref="id12"/>
- <label kind="synchronisation" x="-109" y="-93">pickup_success!</label>
- <nail x="-127" y="-153"/>
- <nail x="136" y="-102"/>
+ <label kind="synchronisation" x="-42" y="-51">pickup_success!</label>
+ <nail x="-17" y="-34"/>
+ <nail x="17" y="-34"/>
</transition>
</template>
<template>
<name x="5" y="5">Cobot</name>
<declaration>// Place local declarations here.</declaration>
- <location id="id13" x="-102" y="42">
- <name x="-112" y="8">waiting</name>
+ <location id="id13" x="-221" y="-119">
</location>
- <location id="id14" x="204" y="68">
- <name x="221" y="68">pick_bottle</name>
- <label kind="invariant" x="127" y="85">orient_constraint_set == false</label>
+ <location id="id14" x="127" y="-119">
+ <name x="34" y="-153">pick_bottle</name>
</location>
- <location id="id15" x="595" y="42">
- <name x="578" y="25">move_to_glass</name>
- <label kind="invariant" x="501" y="59">bottle_in_start_pos == false</label>
+ <location id="id15" x="578" y="-119">
+ <name x="535" y="-110">move_to_glass</name>
+ <label kind="invariant" x="467" y="-93">bottle_in_start_pos == false</label>
</location>
- <location id="id16" x="1020" y="59">
- <name x="926" y="68">start_pour</name>
+ <location id="id16" x="773" y="-8">
+ <name x="679" y="1">start_pour</name>
</location>
- <location id="id17" x="442" y="-119">
- <name x="399" y="-153">shut_down</name>
+ <location id="id17" x="1003" y="-263">
+ <name x="960" y="-297">shut_down</name>
</location>
- <location id="id18" x="918" y="289">
- <name x="867" y="314">stop_pour</name>
+ <location id="id18" x="773" y="110">
+ <name x="688" y="102">stop_pour</name>
</location>
- <location id="id19" x="442" y="289">
- <name x="348" y="263">place_bottle</name>
+ <location id="id19" x="357" y="76">
+ <name x="255" y="51">place_bottle</name>
</location>
- <location id="id20" x="-102" y="399">
- <name x="-102" y="407">pick_glass</name>
- <label kind="invariant" x="-119" y="382">bottle_in_start_pos == true</label>
+ <location id="id20" x="-246" y="76">
+ <name x="-365" y="34">pick_glass</name>
+ <label kind="invariant" x="-459" y="51">bottle_in_start_pos == true</label>
</location>
- <location id="id21" x="833" y="493">
- <name x="790" y="501">place_glass_target</name>
+ <location id="id21" x="314" y="365">
+ <name x="271" y="373">place_glass_target</name>
</location>
- <location id="id22" x="850" y="637">
- <name x="799" y="654">place_glass_orig</name>
+ <location id="id22" x="-246" y="612">
+ <name x="-238" y="570">place_glass_orig</name>
</location>
- <location id="id23" x="1343" y="595">
- <name x="1343" y="612">goto_last_state</name>
+ <location id="id23" x="314" y="552">
+ <name x="314" y="569">goto_last_state</name>
</location>
- <location id="id24" x="663" y="586">
- <name x="501" y="569">goto_last_state_orig</name>
+ <location id="id24" x="1113" y="612">
+ <name x="952" y="620">goto_last_state_orig</name>
</location>
- <location id="id25" x="1275" y="459">
- <name x="1258" y="476">move_to_start_pos</name>
+ <location id="id25" x="977" y="365">
+ <name x="918" y="331">move_to_start_pos</name>
</location>
- <location id="id26" x="1428" y="348">
- <name x="1418" y="314">finished_success</name>
+ <location id="id26" x="977" y="501">
+ <name x="833" y="493">finished_success</name>
</location>
- <location id="id27" x="340" y="8">
+ <location id="id27" x="365" y="-119">
</location>
- <location id="id28" x="858" y="42">
+ <location id="id28" x="773" y="-119">
</location>
- <location id="id29" x="391" y="535">
+ <location id="id29" x="-59" y="365">
</location>
- <location id="id30" x="1224" y="357">
- <label kind="invariant" x="1173" y="348">glass_in_target_pos == 1</label>
+ <location id="id30" x="671" y="365">
+ <label kind="invariant" x="595" y="382">glass_in_target_pos == true</label>
</location>
- <location id="id31" x="102" y="612">
+ <location id="id31" x="-246" y="408">
</location>
- <location id="id32" x="1224" y="544">
- <name x="1156" y="544">stepping_back</name>
+ <location id="id32" x="314" y="467">
+ <name x="340" y="459">stepping_back</name>
</location>
- <init ref="id13"/>
+ <location id="id33" x="-374" y="-119">
+ </location>
+ <location id="id34" x="-25" y="-119">
+ <committed/>
+ </location>
+ <location id="id35" x="-127" y="-221">
+ </location>
+ <location id="id36" x="535" y="76">
+ </location>
+ <location id="id37" x="-25" y="76">
+ </location>
+ <init ref="id33"/>
+ <transition>
+ <source ref="id37"/>
+ <target ref="id20"/>
+ <label kind="synchronisation" x="-153" y="34">goal?</label>
+ <label kind="assignment" x="-221" y="51">orient_constraint_set = false</label>
+ </transition>
+ <transition>
+ <source ref="id36"/>
+ <target ref="id19"/>
+ <label kind="synchronisation" x="416" y="34">goal?</label>
+ <label kind="assignment" x="382" y="51">orient_constraint_set = true</label>
+ </transition>
+ <transition>
+ <source ref="id34"/>
+ <target ref="id14"/>
+ </transition>
+ <transition>
+ <source ref="id35"/>
+ <target ref="id34"/>
+ <label kind="synchronisation" x="-136" y="-195">move_success?</label>
+ <nail x="-25" y="-221"/>
+ </transition>
+ <transition>
+ <source ref="id18"/>
+ <target ref="id18"/>
+ <label kind="synchronisation" x="867" y="102">move_fail?</label>
+ <nail x="858" y="84"/>
+ <nail x="858" y="144"/>
+ </transition>
+ <transition>
+ <source ref="id33"/>
+ <target ref="id35"/>
+ <label kind="select" x="-357" y="-280">i : int[1,15]</label>
+ <label kind="synchronisation" x="-357" y="-263">pressed?</label>
+ <label kind="assignment" x="-459" y="-246">init_retry_count = i, retry_count = init_retry_count</label>
+ <nail x="-374" y="-221"/>
+ </transition>
+ <transition>
+ <source ref="id33"/>
+ <target ref="id13"/>
+ <label kind="synchronisation" x="-348" y="-110">move_success?</label>
+ </transition>
<transition>
<source ref="id32"/>
<target ref="id23"/>
- <label kind="synchronisation" x="1249" y="561">move_success?</label>
+ <label kind="synchronisation" x="323" y="501">move_success?</label>
</transition>
<transition>
<source ref="id31"/>
<target ref="id20"/>
- <label kind="synchronisation" x="-17" y="518">move_goal?</label>
- <label kind="assignment" x="-17" y="535">orient_constraint_set = false</label>
+ <label kind="synchronisation" x="-357" y="298">goal?</label>
+ <label kind="assignment" x="-450" y="315">orient_constraint_set = false</label>
</transition>
<transition>
<source ref="id30"/>
<target ref="id25"/>
- <label kind="synchronisation" x="1224" y="399">move_goal?</label>
- <label kind="assignment" x="1198" y="416">orient_constraint_set = false</label>
+ <label kind="synchronisation" x="782" y="331">goal?</label>
+ <label kind="assignment" x="722" y="348">orient_constraint_set = false</label>
</transition>
<transition>
<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>
+ <label kind="synchronisation" x="-17" y="348">goal?</label>
+ <label kind="assignment" x="-34" y="365">orient_constraint_set = true</label>
</transition>
<transition>
<source ref="id28"/>
<target ref="id16"/>
- <label kind="synchronisation" x="875" y="25">move_goal?</label>
- <label kind="assignment" x="867" y="42">orient_constraint_set = false</label>
+ <label kind="synchronisation" x="773" y="-93">goal?</label>
+ <label kind="assignment" x="773" y="-76">orient_constraint_set = false</label>
</transition>
<transition>
<source ref="id27"/>
<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>
+ <label kind="synchronisation" x="408" y="-170">goal?</label>
+ <label kind="assignment" x="365" y="-153">orient_constraint_set = true</label>
</transition>
<transition>
<source ref="id25"/>
<target ref="id26"/>
- <label kind="synchronisation" x="1334" y="382">move_success?</label>
+ <label kind="synchronisation" x="858" y="450">move_success?</label>
</transition>
<transition>
<source ref="id21"/>
<target ref="id30"/>
- <label kind="synchronisation" x="1054" y="399">place_success?</label>
+ <label kind="synchronisation" x="459" y="365">place_success?</label>
</transition>
<transition>
<source ref="id24"/>
<target ref="id17"/>
- <nail x="714" y="-93"/>
+ <label kind="synchronisation" x="1003" y="561">move_success?</label>
+ <nail x="1113" y="-297"/>
</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>
+ <label kind="guard" x="629" y="578">retry_count == 1</label>
+ <label kind="synchronisation" x="629" 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="synchronisation" x="467" y="433">pickup_success?</label>
- <nail x="603" y="467"/>
- <nail x="663" y="-59"/>
+ <label kind="guard" x="578" y="170">place_glass_retry == 0</label>
+ <label kind="synchronisation" x="578" y="187">pickup_success?</label>
+ <nail x="178" y="204"/>
+ <nail x="1079" y="212"/>
+ <nail x="1079" y="-272"/>
</transition>
<transition>
<source ref="id22"/>
<target ref="id31"/>
- <label kind="synchronisation" x="255" y="629">place_success?</label>
- <label kind="assignment" x="255" y="646">place_glass_retry--</label>
+ <label kind="synchronisation" x="-365" y="493">place_success?</label>
+ <label kind="assignment" x="-382" y="510">place_glass_retry--</label>
</transition>
<transition>
<source ref="id22"/>
<target ref="id22"/>
- <label kind="guard" x="808" y="556">retry_count > 1</label>
- <label kind="synchronisation" x="808" y="573">place_fail?</label>
- <label kind="assignment" x="808" y="590">retry_count--</label>
- <nail x="790" y="595"/>
- <nail x="867" y="586"/>
+ <label kind="guard" x="-441" y="561">retry_count > 1</label>
+ <label kind="synchronisation" x="-441" y="578">place_fail?</label>
+ <label kind="assignment" x="-450" y="595">retry_count--</label>
+ <nail x="-348" y="638"/>
+ <nail x="-348" y="587"/>
</transition>
<transition>
<source ref="id23"/>
<target ref="id22"/>
- <label kind="synchronisation" x="1122" y="612">move_success?</label>
- <label kind="assignment" x="1096" y="629">retry_count = init_retry_count</label>
+ <label kind="synchronisation" x="76" y="552">move_success?</label>
+ <label kind="assignment" x="42" y="586">retry_count = init_retry_count</label>
</transition>
<transition>
<source ref="id22"/>
<target ref="id22"/>
- <label kind="synchronisation" x="979" y="633">move_fail?</label>
- <nail x="1071" y="629"/>
- <nail x="1080" y="663"/>
+ <label kind="synchronisation" x="-204" y="655">move_fail?</label>
+ <nail x="-195" y="638"/>
+ <nail x="-221" y="663"/>
</transition>
<transition>
<source ref="id22"/>
<target ref="id22"/>
- <label kind="synchronisation" x="909" y="680">move_success?</label>
- <nail x="926" y="671"/>
- <nail x="901" y="697"/>
+ <label kind="synchronisation" x="-400" y="664">move_success?</label>
+ <nail x="-264" y="672"/>
+ <nail x="-297" y="655"/>
</transition>
<transition>
<source ref="id21"/>
<target ref="id32"/>
- <label kind="guard" x="960" y="518">retry_count == 1</label>
- <label kind="synchronisation" x="969" y="535">place_fail?</label>
+ <label kind="guard" x="178" y="416">retry_count == 1</label>
+ <label kind="synchronisation" x="187" y="433">place_fail?</label>
</transition>
<transition>
<source ref="id21"/>
<target ref="id21"/>
- <label kind="guard" x="909" y="433">retry_count > 1</label>
- <label kind="synchronisation" x="926" y="450">place_fail?</label>
- <label kind="assignment" x="935" y="467">retry_count--</label>
- <nail x="994" y="450"/>
- <nail x="994" y="510"/>
+ <label kind="guard" x="399" y="297">retry_count > 1</label>
+ <label kind="synchronisation" x="399" y="314">place_fail?</label>
+ <label kind="assignment" x="399" y="331">retry_count--</label>
+ <nail x="391" y="323"/>
+ <nail x="399" y="348"/>
</transition>
<transition>
<source ref="id21"/>
<target ref="id21"/>
- <label kind="synchronisation" x="850" y="408">move_fail?</label>
- <nail x="833" y="416"/>
- <nail x="892" y="433"/>
+ <label kind="synchronisation" x="280" y="297">move_fail?</label>
+ <nail x="280" y="323"/>
+ <nail x="340" y="323"/>
</transition>
<transition>
<source ref="id21"/>
<target ref="id21"/>
- <label kind="synchronisation" x="723" y="442">move_success?</label>
- <nail x="705" y="425"/>
- <nail x="799" y="425"/>
+ <label kind="synchronisation" x="102" y="314">move_success?</label>
+ <nail x="204" y="348"/>
+ <nail x="220" y="314"/>
</transition>
<transition>
<source ref="id20"/>
<target ref="id29"/>
- <label kind="guard" x="212" y="476">place_glass_retry > 0</label>
- <label kind="synchronisation" x="212" y="459">pickup_success?</label>
- <label kind="assignment" x="195" y="493">retry_count = init_retry_count</label>
+ <label kind="guard" x="-178" y="221">place_glass_retry > 0</label>
+ <label kind="synchronisation" x="-170" y="238">pickup_success?</label>
+ <label kind="assignment" x="-161" y="255">retry_count = init_retry_count</label>
</transition>
<transition>
<source ref="id20"/>
<target ref="id17"/>
- <label kind="guard" x="204" y="221">retry_count == 1</label>
- <label kind="synchronisation" x="221" y="238">pickup_fail?</label>
- <nail x="348" y="221"/>
+ <label kind="guard" x="221" y="212">retry_count == 1</label>
+ <label kind="synchronisation" x="221" y="229">pickup_fail?</label>
+ <nail x="178" y="246"/>
+ <nail x="1096" y="255"/>
+ <nail x="1096" y="-280"/>
</transition>
<transition>
<source ref="id20"/>
<target ref="id20"/>
- <label kind="guard" x="-204" y="475">retry_count > 1</label>
- <label kind="synchronisation" x="-204" y="492">pickup_fail?</label>
- <label kind="assignment" x="-204" y="509">retry_count--</label>
- <nail x="-222" y="560"/>
- <nail x="-129" y="585"/>
+ <label kind="guard" x="-476" y="119">retry_count > 1</label>
+ <label kind="synchronisation" x="-476" y="136">pickup_fail?</label>
+ <label kind="assignment" x="-476" y="153">retry_count--</label>
+ <nail x="-373" y="144"/>
+ <nail x="-339" y="169"/>
</transition>
<transition>
<source ref="id20"/>
<target ref="id20"/>
- <label kind="synchronisation" x="-221" y="382">move_fail?</label>
- <nail x="-187" y="382"/>
- <nail x="-195" y="399"/>
+ <label kind="synchronisation" x="-399" y="76">move_fail?</label>
+ <nail x="-323" y="68"/>
+ <nail x="-323" y="93"/>
</transition>
<transition>
<source ref="id20"/>
<target ref="id20"/>
- <label kind="synchronisation" x="-118" y="335">move_success?</label>
- <nail x="-68" y="348"/>
- <nail x="-136" y="356"/>
+ <label kind="synchronisation" x="-289" y="17">move_success?</label>
+ <nail x="-212" y="33"/>
+ <nail x="-280" y="33"/>
</transition>
<transition>
<source ref="id19"/>
- <target ref="id20"/>
- <label kind="synchronisation" x="255" y="297">place_success?</label>
- <label kind="assignment" x="119" y="323">retry_count = init_retry_count, place_glass_retry = init_retry_count</label>
+ <target ref="id37"/>
+ <label kind="synchronisation" x="85" y="76">place_success?</label>
+ <label kind="assignment" x="-102" y="93">retry_count = init_retry_count, place_glass_retry = init_retry_count</label>
</transition>
<transition>
<source ref="id19"/>
<target ref="id17"/>
- <label kind="guard" x="374" y="110">retry_count == 1</label>
- <label kind="synchronisation" x="391" y="93">place_fail?</label>
- <nail x="382" y="161"/>
+ <label kind="guard" x="935" y="144">retry_count == 1</label>
+ <label kind="synchronisation" x="960" y="161">place_fail?</label>
+ <nail x="1062" y="195"/>
+ <nail x="1062" y="-263"/>
</transition>
<transition>
<source ref="id19"/>
<target ref="id19"/>
- <label kind="guard" x="450" y="161">retry_count > 1</label>
- <label kind="synchronisation" x="434" y="182">place_fail?</label>
- <label kind="assignment" x="434" y="199">retry_count--</label>
- <nail x="416" y="195"/>
- <nail x="552" y="204"/>
+ <label kind="guard" x="255" y="-34">retry_count > 1</label>
+ <label kind="synchronisation" x="255" y="-17">place_fail?</label>
+ <label kind="assignment" x="255" y="0">retry_count--</label>
+ <nail x="315" y="17"/>
+ <nail x="408" y="17"/>
</transition>
<transition>
<source ref="id19"/>
<target ref="id19"/>
- <label kind="synchronisation" x="476" y="348">move_success?</label>
- <nail x="459" y="374"/>
- <nail x="510" y="357"/>
+ <label kind="synchronisation" x="374" y="136">move_success?</label>
+ <nail x="383" y="136"/>
+ <nail x="434" y="110"/>
</transition>
<transition>
<source ref="id19"/>
<target ref="id19"/>
- <label kind="synchronisation" x="374" y="382">move_fail?</label>
- <nail x="365" y="399"/>
- <nail x="433" y="408"/>
- <nail x="442" y="306"/>
+ <label kind="synchronisation" x="264" y="119">move_fail?</label>
+ <nail x="298" y="110"/>
+ <nail x="349" y="136"/>
</transition>
<transition>
<source ref="id14"/>
<target ref="id14"/>
- <label kind="synchronisation" x="272" y="136">move_fail?</label>
- <nail x="229" y="153"/>
- <nail x="280" y="161"/>
+ <label kind="synchronisation" x="144" y="-8">move_fail?</label>
+ <nail x="127" y="-8"/>
+ <nail x="178" y="-8"/>
</transition>
<transition>
<source ref="id14"/>
<target ref="id14"/>
- <label kind="synchronisation" x="51" y="102">move_success?</label>
- <nail x="119" y="119"/>
- <nail x="178" y="127"/>
- </transition>
- <transition>
- <source ref="id18"/>
- <target ref="id19"/>
- <label kind="synchronisation" x="595" y="263">move_success?</label>
- <label kind="assignment" x="603" y="289">retry_count = init_retry_count</label>
- </transition>
- <transition>
- <source ref="id18"/>
- <target ref="id17"/>
- <label kind="guard" x="1181" y="0">retry_count == 1</label>
- <label kind="synchronisation" x="1232" y="-25">pickup_fail?</label>
- <nail x="1436" y="-93"/>
+ <label kind="synchronisation" x="-8" y="-8">move_success?</label>
+ <nail x="59" y="-8"/>
+ <nail x="110" y="-8"/>
</transition>
<transition>
<source ref="id18"/>
- <target ref="id18"/>
- <label kind="guard" x="1071" y="323">retry_count > 1</label>
- <label kind="synchronisation" x="1079" y="306">move_fail?</label>
- <label kind="assignment" x="1062" y="340">retry_count--</label>
- <nail x="1096" y="246"/>
- <nail x="1037" y="382"/>
+ <target ref="id36"/>
+ <label kind="synchronisation" x="595" y="42">move_success?</label>
+ <label kind="assignment" x="544" y="59">retry_count = init_retry_count</label>
+ <nail x="722" y="76"/>
</transition>
<transition>
<source ref="id16"/>
<target ref="id18"/>
- <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="synchronisation" x="782" y="42">move_success?</label>
</transition>
<transition>
<source ref="id16"/>
<target ref="id16"/>
- <label kind="synchronisation" x="969" y="-42">move_fail?</label>
- <nail x="918" y="-25"/>
- <nail x="1139" y="-25"/>
+ <label kind="synchronisation" x="875" y="-17">move_fail?</label>
+ <nail x="867" y="-42"/>
+ <nail x="867" y="26"/>
</transition>
<transition>
<source ref="id15"/>
<target ref="id28"/>
- <label kind="synchronisation" x="697" y="25">move_success?</label>
+ <label kind="synchronisation" x="620" y="-136">move_success?</label>
</transition>
<transition>
<source ref="id15"/>
<target ref="id15"/>
- <label kind="synchronisation" x="544" y="-42">move_fail?</label>
- <nail x="518" y="-8"/>
- <nail x="637" y="-25"/>
+ <label kind="synchronisation" x="629" y="-187">move_fail?</label>
+ <nail x="603" y="-170"/>
+ <nail x="637" y="-144"/>
</transition>
<transition>
<source ref="id14"/>
<target ref="id17"/>
- <label kind="guard" x="280" y="-102">retry_count == 1</label>
- <label kind="synchronisation" x="306" y="-119">pickup_fail?</label>
+ <label kind="guard" x="416" y="-255">retry_count == 1</label>
+ <label kind="synchronisation" x="433" y="-238">pickup_fail?</label>
+ <nail x="425" y="-263"/>
</transition>
<transition>
<source ref="id14"/>
<target ref="id27"/>
- <label kind="synchronisation" x="246" y="34">pickup_success?</label>
+ <label kind="synchronisation" x="221" y="-136">pickup_success?</label>
</transition>
<transition>
<source ref="id14"/>
<target ref="id14"/>
- <label kind="guard" x="110" y="-102">retry_count > 1</label>
- <label kind="synchronisation" x="110" y="-85">pickup_fail?</label>
- <label kind="assignment" x="110" y="-68">retry_count--</label>
- <nail x="127" y="-25"/>
- <nail x="289" y="-42"/>
+ <label kind="guard" x="93" y="-263">retry_count > 1</label>
+ <label kind="synchronisation" x="93" y="-246">pickup_fail?</label>
+ <label kind="assignment" x="93" y="-229">retry_count--</label>
+ <nail x="93" y="-212"/>
+ <nail x="170" y="-212"/>
</transition>
<transition>
<source ref="id13"/>
- <target ref="id14"/>
- <label kind="select" x="-272" y="34">i : int[1,5]</label>
- <label kind="synchronisation" x="-272" y="51">pressed?</label>
- <label kind="assignment" x="-272" y="68">init_retry_count = i, retry_count = init_retry_count</label>
+ <target ref="id34"/>
+ <label kind="select" x="-170" y="-119">i : int[1,15]</label>
+ <label kind="synchronisation" x="-170" y="-102">pressed?</label>
+ <label kind="assignment" x="-280" y="-85">init_retry_count = i, retry_count = init_retry_count</label>
</transition>
</template>
<template>
<name>positionupdatecatcher</name>
- <location id="id33" x="-586" y="-170">
+ <location id="id38" x="-544" y="-331">
</location>
- <init ref="id33"/>
+ <init ref="id38"/>
<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"/>
+ <source ref="id38"/>
+ <target ref="id38"/>
+ <label kind="synchronisation" x="-493" y="-340">position_update?</label>
+ <nail x="-501" y="-357"/>
+ <nail x="-501" y="-306"/>
</transition>
</template>
<template>
<name>positionupdate</name>
- <location id="id34" x="0" y="0">
+ <location id="id39" x="0" y="0">
</location>
- <init ref="id34"/>
+ <init ref="id39"/>
<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"/>
+ <source ref="id39"/>
+ <target ref="id39"/>
+ <label kind="synchronisation" x="59" y="-8">position_update!</label>
+ <nail x="51" y="-17"/>
+ <nail x="51" y="17"/>
</transition>
</template>
<template>
- <name>movegoalcatcher</name>
- <location id="id35" x="-603" y="-229">
+ <name>goalcatcher</name>
+ <location id="id40" x="-578" y="-263">
</location>
- <init ref="id35"/>
+ <init ref="id40"/>
<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"/>
+ <source ref="id40"/>
+ <target ref="id40"/>
+ <label kind="synchronisation" x="-535" y="-272">goal?</label>
+ <nail x="-535" y="-280"/>
+ <nail x="-535" y="-246"/>
</transition>
</template>
<system>// Place template instantiations here.
// List one or more processes to be composed into a system.
-system Cobot, press, move, pickup, place, movegoal, movegoalcatcher, positionupdate, positionupdatecatcher, model_positions;
+system Cobot, press, move, pickup, place, goal_, goalcatcher, positionupdate, positionupdatecatcher, model_positions;
</system>
<queries>
<query>
diff --git a/launch/cobot1_mock_test.launch b/launch/cobot1_mock_test.launch
index ddc9405321dfdd73b6075b24eba5a95f4416c582..30caaf1d5c13501eb9019d8c1685ebc0356156a2 100644
--- a/launch/cobot1_mock_test.launch
+++ b/launch/cobot1_mock_test.launch
@@ -19,5 +19,7 @@
<!-- The execution_duration_monitoring sometimes aborts a valid trajectory execution -->
<param name="move_group/trajectory_execution/execution_duration_monitoring" value="false" />
+
+ <node pkg="cobot_1" type="gazebo_real_time.sh" name="gazebo_real_time" output="screen"/>
</launch>
diff --git a/scripts/gazebo_real_time.sh b/scripts/gazebo_real_time.sh
new file mode 100755
index 0000000000000000000000000000000000000000..730b351fc4c64b51f6de0de99fa6a2113d1ca95c
--- /dev/null
+++ b/scripts/gazebo_real_time.sh
@@ -0,0 +1,5 @@
+#!/bin/bash
+
+sleep 5 # make sure gazebo running already
+# running it faster than default apparently breakes it
+# gz physics --update-rate=700 --step-size=0.002
diff --git a/src/tron_adapter.cpp b/src/tron_adapter.cpp
index fdedc99f1fe1ebefad52e8066849fbfcaba66d5f..4d0db18d79e31c7927b5d22f24b1be0dac342cd8 100644
--- a/src/tron_adapter.cpp
+++ b/src/tron_adapter.cpp
@@ -16,6 +16,7 @@
#include <geometry_msgs/Pose.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Vector3.h>
+#include <algorithm> // for std::find
const std::string IP = "127.0.0.1";
const uint16_t PORT = 8080;
@@ -266,6 +267,13 @@ void get_error_msg(int32_t errorcode) {
}
void add_var_to_channel(Channel& chan, bool is_input, std::string var) {
+ bool var_already_declared = false;
+ for (Mapping& map : mappings)
+ if (std::find(map.channel.vars.begin(), map.channel.vars.end(), var) != map.channel.vars.end()) var_already_declared = true;
+ if (var_already_declared) {
+ ROS_INFO("variable %s was already declared to channel %s", var.c_str(), chan.name.c_str());
+ return;
+ }
byte msg[6 + var.length()];
msg[0] = is_input ? ADD_VAR_TO_INPUT : ADD_VAR_TO_OUTPUT;
add_int32_in_network_order(chan.identifier, msg, 1);
@@ -428,7 +436,23 @@ void on_move_goal(const moveit_msgs::MoveGroupActionGoal::ConstPtr &msg){
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);
+ report_now("goal", 1, &is_orientation_constraint_set);
+}
+
+void on_pickup_goal(const moveit_msgs::PickupActionGoal::ConstPtr &msg){
+ int32_t is_orientation_constraint_set;
+ if (msg->goal.path_constraints.orientation_constraints.empty())
+ is_orientation_constraint_set = 0;
+ else is_orientation_constraint_set = 1;
+ report_now("goal", 1, &is_orientation_constraint_set);
+}
+
+void on_place_goal(const moveit_msgs::PlaceActionGoal::ConstPtr &msg) {
+ int32_t is_orientation_constraint_set;
+ if (msg->goal.path_constraints.orientation_constraints.empty())
+ is_orientation_constraint_set = 0;
+ else is_orientation_constraint_set = 1;
+ report_now("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){
@@ -531,11 +555,21 @@ void configuration_phase(ros::NodeHandle& nh){
mappings.push_back(map);
output_subscribers.push_back(nh.subscribe("/position_joint_trajectory_controller/follow_joint_trajectory/result", 10, on_move_result));
- map = Mapping("/move_group/goal", "move_goal", false);
+ map = Mapping("/move_group/goal", "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("/place/goal", "goal", false);
+ map.add_var_to_mapping("orient_constraint_set");
+ mappings.push_back(map);
+ output_subscribers.push_back(nh.subscribe("/place/goal", 10, on_place_goal));
+
+ map = Mapping("/pickup/goal", "goal", false);
+ map.add_var_to_mapping("orient_constraint_set");
+ mappings.push_back(map);
+ output_subscribers.push_back(nh.subscribe("/pickup/goal", 10, on_pickup_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");