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

No commit message 

No commit message
parent cb6f4fe6
Branches
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
cobot1.xml
+ 290
− 250
View file @ e4d184c0
......@@ -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 &gt; 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 &gt; 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 &gt; 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 &gt; 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 &gt; 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 &gt; 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 &gt; 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 &gt; 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 &gt; 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 &gt; 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 &gt; 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 &gt; 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 &gt; 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>
......
launch/cobot1_mock_test.launch
+ 2
− 0
View file @ e4d184c0
......@@ -20,4 +20,6 @@
<!-- 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>
scripts/gazebo_real_time.sh 0 → 100755
+ 5
− 0
View file @ e4d184c0
#!/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
src/tron_adapter.cpp
+ 36
− 2
View file @ e4d184c0
......@@ -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");
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment