diff --git a/cobot1.xml b/cobot1.xml
index 364a50acf5ce97769e234d344cd9815a9c321080..bbbf7c59872bdcacc12dcecfcc06a61d55bce8db 100644
--- a/cobot1.xml
+++ b/cobot1.xml
@@ -12,8 +12,10 @@ chan place_fail, place_success;
chan move_fail, move_success;
chan goal;
chan position_update;
+
chan test_done;
chan intentional_fail;
+urgent chan asap;
const int max_pos_diff = 1000;
const int max_ang_diff = 180;
@@ -34,6 +36,11 @@ chan glass_diff_to_start_ang_update;
chan glass_diff_to_target_pos_update;
chan glass_diff_to_target_ang_update;
+const int bottle_start_pos_tolerance = 5;
+const int bottle_target_pos_tolerance = 5;
+const int glass_start_pos_tolerance = 5;
+const int glass_target_pos_tolerance = 5;
+
bool bottle_in_start_pos = true;
bool bottle_in_target_pos = false;
bool glass_in_start_pos = true;
@@ -60,107 +67,105 @@ bool glass_in_target_pos = false;
</transition>
</template>
<template>
- <name>model_positions</name>
- <location id="id1" x="-1708" y="-1054">
- <name x="-1759" y="-1083">start_pos</name>
- <label kind="invariant" x="-1921" y="-1062">bottle_diff_to_start_pos < 2</label>
- </location>
- <location id="id2" x="-1487" y="-1053">
- <name x="-1470" y="-1062">bottle_moving</name>
- <label kind="invariant" x="-1479" y="-1045">bottle_diff_to_target_pos > 1 && bottle_diff_to_start_pos > 1</label>
- </location>
- <location id="id3" x="-1487" y="-934">
- <name x="-1470" y="-960">bottle_end_pos</name>
- <label kind="invariant" x="-1470" y="-943">bottle_diff_to_target_pos < 2</label>
- </location>
- <location id="id4" x="-1487" y="-824">
- <name x="-1598" y="-815">glass_moving</name>
+ <name>bottle_position</name>
+ <location id="id1" x="-1853" y="-1054">
+ <name x="-1938" y="-1071">start_pos</name>
</location>
- <location id="id5" x="-1223" y="-824">
- <name x="-1317" y="-807">bottle_and_glass_end_pos</name>
+ <location id="id2" x="-1487" y="-1343">
+ <name x="-1504" y="-1377">moving</name>
</location>
- <location id="id6" x="-918" y="-620">
- <name x="-994" y="-603">glass_start_pos_again</name>
- </location>
- <location id="id7" x="-918" y="-1062">
- <name x="-952" y="-1096">glass_picked_up_again</name>
+ <location id="id3" x="-1130" y="-1054">
+ <name x="-1113" y="-1071">target_pos</name>
</location>
<init ref="id1"/>
<transition>
<source ref="id2"/>
<target ref="id1"/>
- <label kind="assignment" x="-1802" y="-1011">bottle_in_start_pos = true</label>
- <nail x="-1606" y="-1003"/>
- </transition>
- <transition>
- <source ref="id7"/>
- <target ref="id5"/>
- <label kind="guard" x="-1232" y="-1105">glass_diff_to_target_pos < 2</label>
- <label kind="assignment" x="-1215" y="-1088">glass_in_target_pos = true</label>
- <nail x="-1224" y="-1062"/>
+ <label kind="guard" x="-1904" y="-1045">bottle_diff_to_start_pos <= bottle_start_pos_tolerance
+&& bottle_diff_to_target_pos > bottle_target_pos_tolerance</label>
+ <label kind="synchronisation" x="-1700" y="-1011">asap!</label>
+ <label kind="assignment" x="-1776" y="-994">bottle_in_start_pos = true</label>
+ <nail x="-1538" y="-1054"/>
</transition>
<transition>
- <source ref="id4"/>
- <target ref="id6"/>
- <label kind="guard" x="-1436" y="-739">glass_diff_to_start_pos < 2</label>
- <label kind="assignment" x="-1411" y="-722">glass_in_start_pos = true</label>
+ <source ref="id3"/>
+ <target ref="id2"/>
+ <label kind="guard" x="-1428" y="-1045">bottle_diff_to_start_pos > bottle_start_pos_tolerance
+&& bottle_diff_to_target_pos > bottle_target_pos_tolerance</label>
+ <label kind="synchronisation" x="-1292" y="-1011">asap!</label>
+ <label kind="assignment" x="-1368" y="-994">bottle_in_target_pos = false</label>
+ <nail x="-1436" y="-1054"/>
</transition>
<transition>
- <source ref="id5"/>
- <target ref="id7"/>
- <label kind="guard" x="-1139" y="-901">glass_diff_to_target_pos > 1</label>
- <label kind="assignment" x="-1156" y="-884">glass_in_target_pos = false</label>
+ <source ref="id2"/>
+ <target ref="id3"/>
+ <label kind="guard" x="-1377" y="-1266">bottle_diff_to_target_pos <= bottle_target_pos_tolerance</label>
+ <label kind="synchronisation" x="-1317" y="-1249">asap!</label>
+ <label kind="assignment" x="-1326" y="-1232">bottle_in_target_pos = true</label>
</transition>
<transition>
- <source ref="id3"/>
+ <source ref="id1"/>
<target ref="id2"/>
- <label kind="assignment" x="-1717" y="-960">bottle_in_target_pos = false</label>
- <nail x="-1555" y="-985"/>
+ <label kind="guard" x="-1989" y="-1309">bottle_diff_to_start_pos > bottle_start_pos_tolerance
+&& bottle_diff_to_target_pos > bottle_target_pos_tolerance</label>
+ <label kind="synchronisation" x="-1768" y="-1275">asap!</label>
+ <label kind="assignment" x="-1836" y="-1258">bottle_in_start_pos = false</label>
</transition>
+ </template>
+ <template>
+ <name>glass_position</name>
+ <location id="id4" x="-697" y="-246">
+ <name x="-782" y="-255">start_pos</name>
+ </location>
+ <location id="id5" x="-331" y="-510">
+ <name x="-348" y="-552">moving</name>
+ </location>
+ <location id="id6" x="59" y="-246">
+ <name x="76" y="-272">end_pos</name>
+ </location>
+ <init ref="id4"/>
<transition>
- <source ref="id6"/>
+ <source ref="id5"/>
<target ref="id4"/>
- <label kind="guard" x="-1683" y="-688">glass_diff_to_start_pos > 1</label>
- <label kind="assignment" x="-1674" y="-671">glass_in_start_pos = false</label>
- <nail x="-1487" y="-629"/>
+ <label kind="guard" x="-731" y="-238">glass_diff_to_start_pos <= glass_start_pos_tolerance
+&& glass_diff_to_target_pos > glass_target_pos_tolerance</label>
+ <label kind="synchronisation" x="-510" y="-204">asap!</label>
+ <label kind="assignment" x="-561" y="-187">glass_in_start_pos = true</label>
+ <nail x="-348" y="-246"/>
</transition>
<transition>
- <source ref="id7"/>
- <target ref="id6"/>
- <label kind="guard" x="-909" y="-850">glass_diff_to_start_pos < 2</label>
- <label kind="assignment" x="-909" y="-833">glass_in_start_pos = true</label>
+ <source ref="id6"/>
+ <target ref="id5"/>
+ <label kind="guard" x="-297" y="-238">glass_diff_to_start_pos > glass_start_pos_tolerance
+&& glass_diff_to_target_pos > glass_target_pos_tolerance</label>
+ <label kind="synchronisation" x="-178" y="-204">asap!</label>
+ <label kind="assignment" x="-255" y="-187">glass_in_target_pos = false</label>
+ <nail x="-306" y="-246"/>
</transition>
<transition>
<source ref="id4"/>
<target ref="id5"/>
- <label kind="guard" x="-1453" y="-858">glass_diff_to_target_pos < 2</label>
- <label kind="assignment" x="-1445" y="-841">glass_in_target_pos = true</label>
+ <label kind="guard" x="-816" y="-484">glass_diff_to_start_pos > glass_start_pos_tolerance
+&& glass_diff_to_target_pos > glass_target_pos_tolerance</label>
+ <label kind="synchronisation" x="-629" y="-450">asap!</label>
+ <label kind="assignment" x="-697" y="-433">glass_in_start_pos = false</label>
</transition>
<transition>
- <source ref="id3"/>
- <target ref="id4"/>
- <label kind="guard" x="-1683" y="-884">glass_diff_to_start_pos > 1</label>
- <label kind="assignment" x="-1683" y="-867">glass_in_start_pos = false</label>
- </transition>
- <transition>
- <source ref="id2"/>
- <target ref="id3"/>
- <label kind="assignment" x="-1479" y="-1003">bottle_in_target_pos = true</label>
- </transition>
- <transition>
- <source ref="id1"/>
- <target ref="id2"/>
- <label kind="assignment" x="-1683" y="-1079">bottle_in_start_pos = false</label>
+ <source ref="id5"/>
+ <target ref="id6"/>
+ <label kind="guard" x="-263" y="-476">glass_diff_to_target_pos <= glass_target_pos_tolerance</label>
+ <label kind="synchronisation" x="-136" y="-459">asap!</label>
+ <label kind="assignment" x="-212" y="-442">glass_in_target_pos = true</label>
</transition>
</template>
<template>
<name>goal_</name>
- <location id="id8" x="0" y="0">
+ <location id="id7" x="0" y="0">
</location>
- <init ref="id8"/>
+ <init ref="id7"/>
<transition>
- <source ref="id8"/>
- <target ref="id8"/>
+ <source ref="id7"/>
+ <target ref="id7"/>
<label kind="synchronisation" x="-42" y="-51">goal!</label>
<nail x="-25" y="-34"/>
<nail x="25" y="-34"/>
@@ -168,32 +173,32 @@ bool glass_in_target_pos = false;
</template>
<template>
<name>press</name>
- <location id="id9" x="-3153" y="-3026">
+ <location id="id8" x="-3153" y="-3026">
</location>
- <location id="id10" x="-3026" y="-3026">
+ <location id="id9" x="-3026" y="-3026">
</location>
- <init ref="id9"/>
+ <init ref="id8"/>
<transition>
- <source ref="id9"/>
- <target ref="id10"/>
+ <source ref="id8"/>
+ <target ref="id9"/>
<label kind="synchronisation" x="-3119" y="-3043">pressed!</label>
</transition>
</template>
<template>
<name>move</name>
- <location id="id11" x="-612" y="-68">
+ <location id="id10" x="-612" y="-68">
</location>
- <init ref="id11"/>
+ <init ref="id10"/>
<transition>
- <source ref="id11"/>
- <target ref="id11"/>
+ <source ref="id10"/>
+ <target ref="id10"/>
<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"/>
+ <source ref="id10"/>
+ <target ref="id10"/>
<label kind="synchronisation" x="-663" y="-119">move_success!</label>
<nail x="-629" y="-102"/>
<nail x="-595" y="-102"/>
@@ -201,24 +206,37 @@ bool glass_in_target_pos = false;
</template>
<template>
<name>pickup</name>
- <location id="id12" x="0" y="0">
+ <location id="id11" x="0" y="0">
</location>
- <init ref="id12"/>
+ <init ref="id11"/>
<transition>
- <source ref="id12"/>
- <target ref="id12"/>
+ <source ref="id11"/>
+ <target ref="id11"/>
<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"/>
+ <source ref="id11"/>
+ <target ref="id11"/>
<label kind="synchronisation" x="-42" y="-51">pickup_success!</label>
<nail x="-17" y="-34"/>
<nail x="17" y="-34"/>
</transition>
</template>
+ <template>
+ <name>asapcatcher</name>
+ <location id="id12" x="-272" y="-119">
+ </location>
+ <init ref="id12"/>
+ <transition>
+ <source ref="id12"/>
+ <target ref="id12"/>
+ <label kind="synchronisation" x="-289" y="-187">asap?</label>
+ <nail x="-306" y="-161"/>
+ <nail x="-238" y="-161"/>
+ </transition>
+ </template>
<template>
<name x="5" y="5">Cobot</name>
<declaration>// Place local declarations here.</declaration>
@@ -229,13 +247,13 @@ bool glass_in_target_pos = false;
</location>
<location id="id15" x="578" y="-119">
<name x="535" y="-110">moving_to_glass</name>
- <label kind="invariant" x="467" y="-93">bottle_in_start_pos == false</label>
+ <label kind="invariant" x="484" y="-93">bottle_in_start_pos == false</label>
</location>
<location id="id16" x="773" y="-8">
<name x="680" y="-17">start_pour</name>
</location>
- <location id="id17" x="1003" y="-263">
- <name x="960" y="-297">shut_down</name>
+ <location id="id17" x="833" y="-263">
+ <name x="790" y="-297">shut_down</name>
</location>
<location id="id18" x="773" y="76">
<name x="748" y="85">stop_pour</name>
@@ -244,8 +262,7 @@ bool glass_in_target_pos = false;
<name x="246" y="51">placing_bottle</name>
</location>
<location id="id20" x="-246" y="76">
- <name x="-374" y="34">picking_glass</name>
- <label kind="invariant" x="-467" y="51">bottle_in_target_pos == true</label>
+ <name x="-382" y="42">picking_glass</name>
</location>
<location id="id21" x="314" y="365">
<name x="246" y="374">placing_glass_target</name>
@@ -257,13 +274,13 @@ bool glass_in_target_pos = false;
<name x="314" y="569">goto_last_state</name>
</location>
<location id="id24" x="1113" y="612">
- <name x="892" y="629">goto_last_state_before_shut_down</name>
+ <name x="884" y="629">goto_last_state_before_shut_down</name>
</location>
<location id="id25" x="977" y="365">
<name x="918" y="331">move_to_start_pos</name>
</location>
- <location id="id26" x="977" y="527">
- <name x="841" y="518">finished_success</name>
+ <location id="id26" x="977" y="544">
+ <name x="841" y="535">finished_success</name>
</location>
<location id="id27" x="365" y="-119">
<name x="323" y="-102">bottle_picked</name>
@@ -293,25 +310,44 @@ bool glass_in_target_pos = false;
<location id="id35" x="-127" y="-221">
</location>
<location id="id36" x="535" y="76">
- <name x="535" y="85">pouring_done</name>
+ <name x="552" y="76">pouring_done</name>
+ <label kind="invariant" x="527" y="93">glass_in_start_pos == true</label>
</location>
<location id="id37" x="-25" y="76">
- <name x="-17" y="51">bottle_placed</name>
+ <name x="-17" y="25">bottle_placed</name>
+ <label kind="invariant" x="-17" y="42">bottle_in_target_pos == true</label>
</location>
- <location id="id38" x="977" y="459">
- <name x="875" y="442">in_start_pos</name>
- <label kind="invariant" x="527" y="459">glass_in_target_pos == true && bottle_in_target_pos == true</label>
+ <location id="id38" x="977" y="433">
+ <name x="994" y="424">in_start_pos</name>
+ <label kind="invariant" x="731" y="407">glass_in_target_pos == true
+&& bottle_in_target_pos == true</label>
</location>
<location id="id39" x="663" y="612">
<name x="578" y="629">steping_back_start</name>
</location>
<location id="id40" x="1062" y="-187">
</location>
+ <location id="id41" x="1062" y="-238">
+ <committed/>
+ </location>
+ <location id="id42" x="977" y="493">
+ <committed/>
+ </location>
<init ref="id33"/>
+ <transition>
+ <source ref="id42"/>
+ <target ref="id26"/>
+ <label kind="synchronisation" x="986" y="510">test_done!</label>
+ </transition>
<transition>
<source ref="id40"/>
+ <target ref="id41"/>
+ <label kind="synchronisation" x="1011" y="-221">asap!</label>
+ </transition>
+ <transition>
+ <source ref="id41"/>
<target ref="id17"/>
- <label kind="synchronisation" x="1028" y="-246">test_done!</label>
+ <label kind="synchronisation" x="952" y="-280">test_done!</label>
<nail x="1062" y="-263"/>
</transition>
<transition>
@@ -321,8 +357,8 @@ bool glass_in_target_pos = false;
</transition>
<transition>
<source ref="id38"/>
- <target ref="id26"/>
- <label kind="synchronisation" x="892" y="484">test_done!</label>
+ <target ref="id42"/>
+ <label kind="synchronisation" x="986" y="450">asap!</label>
</transition>
<transition>
<source ref="id37"/>
@@ -334,7 +370,7 @@ bool glass_in_target_pos = false;
<source ref="id36"/>
<target ref="id19"/>
<label kind="synchronisation" x="433" y="34">goal?</label>
- <label kind="assignment" x="382" y="51">orient_constraint_set = true</label>
+ <label kind="assignment" x="374" y="51">orient_constraint_set = true</label>
</transition>
<transition>
<source ref="id34"/>
@@ -357,7 +393,7 @@ bool glass_in_target_pos = false;
<transition>
<source ref="id33"/>
<target ref="id13"/>
- <label kind="synchronisation" x="-348" y="-110">move_success?</label>
+ <label kind="synchronisation" x="-348" y="-119">move_success?</label>
</transition>
<transition>
<source ref="id32"/>
@@ -397,8 +433,8 @@ bool glass_in_target_pos = false;
<transition>
<source ref="id25"/>
<target ref="id38"/>
- <label kind="synchronisation" x="867" y="408">move_success?</label>
- <nail x="977" y="416"/>
+ <label kind="synchronisation" x="977" y="391">move_success?</label>
+ <nail x="977" y="407"/>
</transition>
<transition>
<source ref="id21"/>
@@ -487,8 +523,8 @@ bool glass_in_target_pos = false;
<source ref="id21"/>
<target ref="id21"/>
<label kind="synchronisation" x="102" y="314">move_success?</label>
- <nail x="204" y="348"/>
- <nail x="220" y="314"/>
+ <nail x="221" y="340"/>
+ <nail x="229" y="323"/>
</transition>
<transition>
<source ref="id20"/>
@@ -533,7 +569,7 @@ bool glass_in_target_pos = false;
<source ref="id19"/>
<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>
+ <label kind="assignment" x="-127" y="93">retry_count = init_retry_count, place_glass_retry = init_retry_count</label>
</transition>
<transition>
<source ref="id19"/>
@@ -599,10 +635,9 @@ bool glass_in_target_pos = false;
<transition>
<source ref="id14"/>
<target ref="id40"/>
- <label kind="guard" x="450" y="-272">retry_count == 1</label>
- <label kind="synchronisation" x="450" y="-255">pickup_fail?</label>
- <nail x="408" y="-272"/>
- <nail x="858" y="-272"/>
+ <label kind="guard" x="450" y="-229">retry_count == 1</label>
+ <label kind="synchronisation" x="467" y="-212">pickup_fail?</label>
+ <nail x="374" y="-187"/>
</transition>
<transition>
<source ref="id14"/>
@@ -628,12 +663,12 @@ bool glass_in_target_pos = false;
</template>
<template>
<name>testdonecatcher</name>
- <location id="id41" x="0" y="0">
+ <location id="id43" x="0" y="0">
</location>
- <init ref="id41"/>
+ <init ref="id43"/>
<transition>
- <source ref="id41"/>
- <target ref="id41"/>
+ <source ref="id43"/>
+ <target ref="id43"/>
<label kind="synchronisation" x="8" y="-42">test_done?</label>
<nail x="59" y="-17"/>
<nail x="59" y="17"/>
@@ -641,12 +676,12 @@ bool glass_in_target_pos = false;
</template>
<template>
<name>positionupdatecatcher</name>
- <location id="id42" x="-731" y="-544">
+ <location id="id44" x="-731" y="-544">
</location>
- <init ref="id42"/>
+ <init ref="id44"/>
<transition>
- <source ref="id42"/>
- <target ref="id42"/>
+ <source ref="id44"/>
+ <target ref="id44"/>
<label kind="select" x="-1181" y="-816">i : int[0,max_ang_diff]</label>
<label kind="synchronisation" x="-1181" y="-833">glass_diff_to_start_ang_update?</label>
<label kind="assignment" x="-1181" y="-799">glass_diff_to_start_ang = i</label>
@@ -654,8 +689,8 @@ bool glass_in_target_pos = false;
<nail x="-935" y="-799"/>
</transition>
<transition>
- <source ref="id42"/>
- <target ref="id42"/>
+ <source ref="id44"/>
+ <target ref="id44"/>
<label kind="select" x="-450" y="-790">i : int[0,max_ang_diff]</label>
<label kind="synchronisation" x="-476" y="-807">glass_diff_to_target_ang_update?</label>
<label kind="assignment" x="-442" y="-773">glass_diff_to_target_ang = i</label>
@@ -663,8 +698,8 @@ bool glass_in_target_pos = false;
<nail x="-484" y="-790"/>
</transition>
<transition>
- <source ref="id42"/>
- <target ref="id42"/>
+ <source ref="id44"/>
+ <target ref="id44"/>
<label kind="select" x="-493" y="-382">i : int[0,max_ang_diff]</label>
<label kind="synchronisation" x="-493" y="-399">bottle_diff_to_start_ang_update?</label>
<label kind="assignment" x="-493" y="-365">bottle_diff_to_start_ang = i</label>
@@ -672,8 +707,8 @@ bool glass_in_target_pos = false;
<nail x="-510" y="-382"/>
</transition>
<transition>
- <source ref="id42"/>
- <target ref="id42"/>
+ <source ref="id44"/>
+ <target ref="id44"/>
<label kind="select" x="-1198" y="-416">i : int[0,max_ang_diff]</label>
<label kind="synchronisation" x="-1232" y="-433">bottle_diff_to_target_ang_update?</label>
<label kind="assignment" x="-1207" y="-399">bottle_diff_to_target_ang = i</label>
@@ -681,8 +716,8 @@ bool glass_in_target_pos = false;
<nail x="-952" y="-391"/>
</transition>
<transition>
- <source ref="id42"/>
- <target ref="id42"/>
+ <source ref="id44"/>
+ <target ref="id44"/>
<label kind="select" x="-858" y="-306">i : int[0,max_pos_diff]</label>
<label kind="synchronisation" x="-858" y="-323">bottle_diff_to_target_pos_update?</label>
<label kind="assignment" x="-858" y="-289">bottle_diff_to_target_pos = i</label>
@@ -690,8 +725,8 @@ bool glass_in_target_pos = false;
<nail x="-697" y="-331"/>
</transition>
<transition>
- <source ref="id42"/>
- <target ref="id42"/>
+ <source ref="id44"/>
+ <target ref="id44"/>
<label kind="select" x="-1232" y="-561">i : int[0,max_pos_diff]</label>
<label kind="synchronisation" x="-1232" y="-578">glass_diff_to_start_pos_update?</label>
<label kind="assignment" x="-1232" y="-544">glass_diff_to_start_pos = i</label>
@@ -699,8 +734,8 @@ bool glass_in_target_pos = false;
<nail x="-1003" y="-527"/>
</transition>
<transition>
- <source ref="id42"/>
- <target ref="id42"/>
+ <source ref="id44"/>
+ <target ref="id44"/>
<label kind="select" x="-773" y="-884">i : int[0,max_pos_diff]</label>
<label kind="synchronisation" x="-824" y="-901">glass_diff_to_target_pos_update?</label>
<label kind="assignment" x="-824" y="-867">glass_diff_to_target_pos = i</label>
@@ -708,8 +743,8 @@ bool glass_in_target_pos = false;
<nail x="-765" y="-850"/>
</transition>
<transition>
- <source ref="id42"/>
- <target ref="id42"/>
+ <source ref="id44"/>
+ <target ref="id44"/>
<label kind="select" x="-323" y="-569">i : int[0,max_pos_diff]</label>
<label kind="synchronisation" x="-323" y="-586">bottle_diff_to_start_pos_update?</label>
<label kind="assignment" x="-323" y="-552">bottle_diff_to_start_pos = i</label>
@@ -719,61 +754,61 @@ bool glass_in_target_pos = false;
</template>
<template>
<name>positionupdate</name>
- <location id="id43" x="0" y="0">
+ <location id="id45" x="0" y="0">
</location>
- <init ref="id43"/>
+ <init ref="id45"/>
<transition>
- <source ref="id43"/>
- <target ref="id43"/>
+ <source ref="id45"/>
+ <target ref="id45"/>
<label kind="synchronisation" x="-331" y="59">bottle_diff_to_target_ang_update!</label>
<nail x="-119" y="42"/>
<nail x="-85" y="68"/>
</transition>
<transition>
- <source ref="id43"/>
- <target ref="id43"/>
+ <source ref="id45"/>
+ <target ref="id45"/>
<label kind="synchronisation" x="-323" y="-110">glass_diff_to_start_ang_update!</label>
<nail x="-85" y="-110"/>
<nail x="-119" y="-85"/>
</transition>
<transition>
- <source ref="id43"/>
- <target ref="id43"/>
+ <source ref="id45"/>
+ <target ref="id45"/>
<label kind="synchronisation" x="119" y="-76">glass_diff_to_target_ang_update!</label>
<nail x="110" y="-42"/>
<nail x="93" y="-68"/>
</transition>
<transition>
- <source ref="id43"/>
- <target ref="id43"/>
+ <source ref="id45"/>
+ <target ref="id45"/>
<label kind="synchronisation" x="102" y="59">bottle_diff_to_start_ang_update!</label>
<nail x="85" y="85"/>
<nail x="102" y="68"/>
</transition>
<transition>
- <source ref="id43"/>
- <target ref="id43"/>
+ <source ref="id45"/>
+ <target ref="id45"/>
<label kind="synchronisation" x="-340" y="-17">glass_diff_to_start_pos_update!</label>
<nail x="-119" y="-25"/>
<nail x="-119" y="8"/>
</transition>
<transition>
- <source ref="id43"/>
- <target ref="id43"/>
+ <source ref="id45"/>
+ <target ref="id45"/>
<label kind="synchronisation" x="-136" y="93">bottle_diff_to_target_pos_update!</label>
<nail x="-34" y="85"/>
<nail x="17" y="85"/>
</transition>
<transition>
- <source ref="id43"/>
- <target ref="id43"/>
+ <source ref="id45"/>
+ <target ref="id45"/>
<label kind="synchronisation" x="-34" y="-119">glass_diff_to_target_pos_update!</label>
<nail x="25" y="-102"/>
<nail x="-25" y="-102"/>
</transition>
<transition>
- <source ref="id43"/>
- <target ref="id43"/>
+ <source ref="id45"/>
+ <target ref="id45"/>
<label kind="synchronisation" x="127" y="-8">bottle_diff_to_start_pos_update!</label>
<nail x="119" y="-17"/>
<nail x="119" y="17"/>
@@ -781,12 +816,12 @@ bool glass_in_target_pos = false;
</template>
<template>
<name>goalcatcher</name>
- <location id="id44" x="-578" y="-263">
+ <location id="id46" x="-578" y="-263">
</location>
- <init ref="id44"/>
+ <init ref="id46"/>
<transition>
- <source ref="id44"/>
- <target ref="id44"/>
+ <source ref="id46"/>
+ <target ref="id46"/>
<label kind="synchronisation" x="-535" y="-272">goal?</label>
<nail x="-535" y="-280"/>
<nail x="-535" y="-246"/>
@@ -794,7 +829,7 @@ bool glass_in_target_pos = false;
</template>
<system>// Place template instantiations here.
// List one or more processes to be composed into a system.
-system Cobot, press, move, pickup, place, goal_, goalcatcher, positionupdate, positionupdatecatcher, model_positions, testdonecatcher;
+system Cobot, press, move, pickup, place, goal_, goalcatcher, positionupdate, positionupdatecatcher, bottle_position, glass_position, testdonecatcher, asapcatcher;
</system>
<queries>
<query>
diff --git a/scripts/gazebo_real_time.sh b/scripts/gazebo_real_time.sh
deleted file mode 100755
index 730b351fc4c64b51f6de0de99fa6a2113d1ca95c..0000000000000000000000000000000000000000
--- a/scripts/gazebo_real_time.sh
+++ /dev/null
@@ -1,5 +0,0 @@
-#!/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/scripts/tron_testing_auto.sh b/scripts/tron_testing_auto.sh
index 8d21b445a54fcf5231db0a9d7c13db03694112e6..0bd6aff7f6ddf2ccec5a694e5c5313211c5f1433 100755
--- a/scripts/tron_testing_auto.sh
+++ b/scripts/tron_testing_auto.sh
@@ -17,7 +17,6 @@ tron_args=( # do not set -o here
"-I SocketAdapter"
"-v 26"
"-q"
- "-P eager"
$model_location # needs to be specified before port
"-- 8080"
)
diff --git a/src/tron_adapter.cpp b/src/tron_adapter.cpp
index a1b17404fc69a6ba90c2831fbca8e7aa4941fc16..ec1eb0bc7fdec1b6a15ffacb53a12e395a38231b 100644
--- a/src/tron_adapter.cpp
+++ b/src/tron_adapter.cpp
@@ -470,14 +470,19 @@ int32_t get_pos_diff_int32(geometry_msgs::Point p1, geometry_msgs::Point p2, int
double pos_diff = get_pos_diff(p1, p2);
return std::round(pos_diff * 100);
}
-double get_ang_diff(geometry_msgs::Quaternion q_msg1, geometry_msgs::Quaternion q_msg2) {
- tf2::Quaternion q1, q2;
+double get_ang_diff_ignoring_yaw(geometry_msgs::Quaternion q_msg1, geometry_msgs::Quaternion q_msg2) {
+ tf2::Quaternion q1, q2, q_diff;
tf2::fromMsg(q_msg1, q1);
tf2::fromMsg(q_msg2, q2);
- return q1.angleShortestPath(q2);
+ q_diff = q1 * q2.inverse();
+ tf2::Matrix3x3 matrix(q_diff);
+ double roll, pitch, yaw;
+ matrix.getRPY(roll, pitch, yaw);
+ q_diff.setRPY(roll, pitch, 0); // set yaw to zero since it doesnt matter for glass or bottle
+ return q_diff.getAngleShortestPath();
}
-int32_t get_ang_diff_int32_deg(geometry_msgs::Quaternion q_msg1, geometry_msgs::Quaternion q_msg2){
- return std::round(get_ang_diff(q_msg1, q_msg2) * 180 / M_PI);
+int32_t get_ang_diff_ignoring_yaw_int32_deg(geometry_msgs::Quaternion q_msg1, geometry_msgs::Quaternion q_msg2){
+ return std::round(get_ang_diff_ignoring_yaw(q_msg1, q_msg2) * 180 / M_PI);
}
const std::vector<std::string> tron_pos_vars_ordered = {
@@ -502,13 +507,13 @@ void on_gazebo_link_states(const gazebo_msgs::LinkStates::ConstPtr &msg){
if (bottle_index == -1 || glass_index == -1) return;
int32_t vars[tron_pos_vars_ordered.size()];
vars[0] = get_pos_diff_int32(msg->pose[bottle_index].position, pose_to_compare_against[0].position);
- vars[1] = get_ang_diff_int32_deg(msg->pose[bottle_index].orientation, pose_to_compare_against[0].orientation);
+ vars[1] = get_ang_diff_ignoring_yaw_int32_deg(msg->pose[bottle_index].orientation, pose_to_compare_against[0].orientation);
vars[2] = get_pos_diff_int32(msg->pose[bottle_index].position, pose_to_compare_against[1].position);
- vars[3] = get_ang_diff_int32_deg(msg->pose[bottle_index].orientation, pose_to_compare_against[1].orientation);
+ vars[3] = get_ang_diff_ignoring_yaw_int32_deg(msg->pose[bottle_index].orientation, pose_to_compare_against[1].orientation);
vars[4] = get_pos_diff_int32(msg->pose[glass_index].position, pose_to_compare_against[2].position);
- vars[5] = get_ang_diff_int32_deg(msg->pose[glass_index].orientation, pose_to_compare_against[2].orientation);
+ vars[5] = get_ang_diff_ignoring_yaw_int32_deg(msg->pose[glass_index].orientation, pose_to_compare_against[2].orientation);
vars[6] = get_pos_diff_int32(msg->pose[glass_index].position, pose_to_compare_against[3].position);
- vars[7] = get_ang_diff_int32_deg(msg->pose[glass_index].orientation, pose_to_compare_against[3].orientation);
+ vars[7] = get_ang_diff_ignoring_yaw_int32_deg(msg->pose[glass_index].orientation, pose_to_compare_against[3].orientation);
for (Mapping& map : mappings) {
if (map.topic == "/gazebo/link_states" && !map.channel.is_input) {
for (int i = 0; i < tron_pos_vars_ordered.size(); i++) {