From 420ab135cef94168e6e0d332af802aa94bda7b77 Mon Sep 17 00:00:00 2001
From: CS <christoph.schroeter1@mailbox.tu-dresden.de>
Date: Sun, 8 Aug 2021 00:07:36 +0200
Subject: [PATCH]
---
cobot1.xml | 479 ++++++++++++++++++++++++++++++-------------
src/tron_adapter.cpp | 101 ++++++++-
2 files changed, 428 insertions(+), 152 deletions(-)
diff --git a/cobot1.xml b/cobot1.xml
index 3fe4ed2..48ec2e8 100644
--- a/cobot1.xml
+++ b/cobot1.xml
@@ -5,10 +5,16 @@
int retry_count = 0;
int init_retry_count = 0;
int place_glass_retry = 0;
+int orient_constraint_set = 0;
chan pressed;
chan pickup_fail, pickup_success;
+chan place_fail, place_success;
chan move_fail, move_success;
-chan place_fail, place_success;</declaration>
+chan move_goal;
+chan position_update;
+int bottle_in_start_pos = 1;
+int glass_in_start_pos = 1;
+int glass_in_target_pos = 0;</declaration>
<template>
<name>place</name>
<location id="id0" x="0" y="0">
@@ -30,33 +36,126 @@ chan place_fail, place_success;</declaration>
</transition>
</template>
<template>
- <name>press</name>
- <location id="id1" x="-3153" y="-3026">
+ <name>model_positions</name>
+ <location id="id1" x="-1547" y="-450">
+ <name x="-1598" y="-479">start_pos</name>
+ </location>
+ <location id="id2" x="-1096" y="-285">
+ <name x="-1130" y="-331">bottle_moving</name>
+ </location>
+ <location id="id3" x="-1504" y="-242">
+ <name x="-1572" y="-280">bottle_end_pos</name>
+ </location>
+ <location id="id4" x="-1538" y="-85">
+ <name x="-1649" y="-76">glass_moving</name>
+ </location>
+ <location id="id5" x="-994" y="-221">
+ <name x="-1045" y="-246">bottle_and_glass_end_pos</name>
+ </location>
+ <location id="id6" x="-748" y="51">
</location>
- <location id="id2" x="-3017" y="-2983">
+ <location id="id7" x="-697" y="-127">
+ <name x="-680" y="-144">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"/>
+ </transition>
+ <transition>
+ <source ref="id5"/>
+ <target ref="id7"/>
+ <label kind="synchronisation" x="-858" y="-195">position_update?</label>
+ <label kind="assignment" x="-892" y="-178">glass_in_target_pos = 0</label>
+ </transition>
+ <transition>
+ <source ref="id3"/>
+ <target ref="id2"/>
+ <label kind="synchronisation" x="-1487" y="-340">position_update?</label>
+ <label kind="assignment" x="-1479" y="-323">bottle_in_start_pos = 0</label>
+ <nail x="-1386" y="-330"/>
+ </transition>
+ <transition>
+ <source ref="id6"/>
+ <target ref="id4"/>
+ <label kind="synchronisation" x="-1147" y="34">position_update?</label>
+ <label kind="assignment" x="-1164" y="51">glass_in_start_pos = 0</label>
+ </transition>
+ <transition>
+ <source ref="id7"/>
+ <target ref="id6"/>
+ <label kind="synchronisation" x="-697" y="-42">position_update?</label>
+ <label kind="assignment" x="-705" y="-25">glass_in_start_pos = 1</label>
+ </transition>
+ <transition>
+ <source ref="id4"/>
+ <target ref="id5"/>
+ <label kind="synchronisation" x="-1241" y="-161">position_update?</label>
+ <label kind="assignment" x="-1266" y="-144">glass_in_target_pos = 1</label>
+ </transition>
+ <transition>
+ <source ref="id3"/>
+ <target ref="id4"/>
+ <label kind="synchronisation" x="-1674" y="-195">position_update?</label>
+ <label kind="assignment" x="-1700" y="-170">glass_in_start_pos = 0</label>
+ </transition>
+ <transition>
+ <source ref="id2"/>
+ <target ref="id3"/>
+ <label kind="synchronisation" x="-1249" y="-272">position_update?</label>
+ <label kind="assignment" x="-1292" y="-255">bottle_in_start_pos = 1</label>
+ </transition>
<transition>
<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>
+ </transition>
+ </template>
+ <template>
+ <name>movegoal</name>
+ <location id="id8" x="0" y="0">
+ </location>
+ <init ref="id8"/>
+ <transition>
+ <source ref="id8"/>
+ <target ref="id8"/>
+ <label kind="synchronisation" x="-136" y="-102">move_goal!</label>
+ <nail x="-178" y="-187"/>
+ <nail x="178" y="-136"/>
+ </transition>
+ </template>
+ <template>
+ <name>press</name>
+ <location id="id9" x="-3153" y="-3026">
+ </location>
+ <location id="id10" x="-3017" y="-2983">
+ </location>
+ <init ref="id9"/>
+ <transition>
+ <source ref="id9"/>
+ <target ref="id10"/>
<label kind="synchronisation" x="-3119" y="-3034">pressed!</label>
</transition>
</template>
<template>
<name>move</name>
- <location id="id3" x="-612" y="-68">
+ <location id="id11" x="-612" y="-68">
</location>
- <init ref="id3"/>
+ <init ref="id11"/>
<transition>
- <source ref="id3"/>
- <target ref="id3"/>
+ <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"/>
</transition>
<transition>
- <source ref="id3"/>
- <target ref="id3"/>
+ <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"/>
@@ -64,19 +163,19 @@ chan place_fail, place_success;</declaration>
</template>
<template>
<name>pickup</name>
- <location id="id4" x="0" y="0">
+ <location id="id12" x="0" y="0">
</location>
- <init ref="id4"/>
+ <init ref="id12"/>
<transition>
- <source ref="id4"/>
- <target ref="id4"/>
+ <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"/>
</transition>
<transition>
- <source ref="id4"/>
- <target ref="id4"/>
+ <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"/>
@@ -85,88 +184,138 @@ chan place_fail, place_success;</declaration>
<template>
<name x="5" y="5">Cobot</name>
<declaration>// Place local declarations here.</declaration>
- <location id="id5" x="-102" y="42">
+ <location id="id13" x="-102" y="42">
<name x="-112" y="8">waiting</name>
</location>
- <location id="id6" x="204" y="68">
- <name x="229" y="76">pick_bottle</name>
+ <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>
- <location id="id7" x="595" y="42">
- <name x="544" y="59">move_to_glass</name>
+ <location id="id15" x="595" y="42">
+ <name x="578" y="25">move_to_glass</name>
</location>
- <location id="id8" x="1020" y="59">
+ <location id="id16" x="1020" y="59">
<name x="926" y="68">start_pour</name>
</location>
- <location id="id9" x="442" y="-119">
+ <location id="id17" x="442" y="-119">
<name x="399" y="-153">shut_down</name>
</location>
- <location id="id10" x="918" y="289">
+ <location id="id18" x="918" y="289">
<name x="867" y="314">stop_pour</name>
</location>
- <location id="id11" x="442" y="289">
+ <location id="id19" x="442" y="289">
<name x="348" y="263">place_bottle</name>
</location>
- <location id="id12" x="17" y="357">
- <name x="17" y="365">pick_glass</name>
+ <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>
- <location id="id13" x="833" y="493">
+ <location id="id21" x="833" y="493">
<name x="790" y="501">place_glass_target</name>
</location>
- <location id="id14" x="850" y="637">
+ <location id="id22" x="850" y="637">
<name x="799" y="654">place_glass_orig</name>
</location>
- <location id="id15" x="1105" y="561">
- <name x="1096" y="527">goto_last_state</name>
+ <location id="id23" x="1343" y="595">
+ <name x="1343" y="612">goto_last_state</name>
</location>
- <location id="id16" x="663" y="586">
+ <location id="id24" x="663" y="586">
<name x="501" y="569">goto_last_state_orig</name>
</location>
- <location id="id17" x="1156" y="391">
- <name x="1105" y="365">move_to_start_pos</name>
+ <location id="id25" x="1275" y="459">
+ <name x="1258" y="476">move_to_start_pos</name>
+ </location>
+ <location id="id26" x="1428" y="348">
+ <name x="1418" y="314">finished_success</name>
+ </location>
+ <location id="id27" x="340" y="8">
+ </location>
+ <location id="id28" x="858" y="42">
</location>
- <location id="id18" x="1317" y="425">
- <name x="1307" y="391">finished_success</name>
+ <location id="id29" x="391" y="535">
</location>
- <init ref="id5"/>
+ <location id="id30" x="1224" y="357">
+ <label kind="invariant" x="1173" y="348">glass_in_target_pos == 1</label>
+ </location>
+ <location id="id31" x="102" y="612">
+ </location>
+ <location id="id32" x="1224" y="544">
+ <name x="1156" y="544">stepping_back</name>
+ </location>
+ <init ref="id13"/>
<transition>
- <source ref="id17"/>
- <target ref="id18"/>
- <label kind="synchronisation" x="1190" y="391">move_success?</label>
+ <source ref="id32"/>
+ <target ref="id23"/>
+ <label kind="synchronisation" x="1249" y="561">move_success?</label>
</transition>
<transition>
- <source ref="id13"/>
- <target ref="id17"/>
- <label kind="synchronisation" x="1028" y="408">place_success?</label>
+ <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>
</transition>
<transition>
- <source ref="id16"/>
- <target ref="id9"/>
- <nail x="714" y="-93"/>
+ <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>
</transition>
<transition>
- <source ref="id14"/>
+ <source ref="id29"/>
+ <target ref="id21"/>
+ <label kind="synchronisation" x="493" y="493">move_goal?</label>
+ <label kind="assignment" x="425" y="510">orient_constraint_set = true</label>
+ </transition>
+ <transition>
+ <source ref="id28"/>
<target ref="id16"/>
- <label kind="guard" x="681" y="577">retry_count == 1</label>
- <label kind="synchronisation" x="681" y="594">place_fail?</label>
+ <label kind="synchronisation" x="875" y="25">move_goal?</label>
+ <label kind="assignment" x="867" y="42">orient_constraint_set = false</label>
</transition>
<transition>
- <source ref="id12"/>
- <target ref="id9"/>
+ <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>
+ </transition>
+ <transition>
+ <source ref="id25"/>
+ <target ref="id26"/>
+ <label kind="synchronisation" x="1334" y="382">move_success?</label>
+ </transition>
+ <transition>
+ <source ref="id21"/>
+ <target ref="id30"/>
+ <label kind="synchronisation" x="1054" y="399">place_success?</label>
+ </transition>
+ <transition>
+ <source ref="id24"/>
+ <target ref="id17"/>
+ <nail x="714" y="-93"/>
+ </transition>
+ <transition>
+ <source ref="id22"/>
+ <target ref="id24"/>
+ <label kind="guard" x="671" y="578">retry_count == 1</label>
+ <label kind="synchronisation" x="697" y="595">place_fail?</label>
+ </transition>
+ <transition>
+ <source ref="id20"/>
+ <target ref="id17"/>
<label kind="guard" x="459" y="416">place_glass_retry == 0</label>
<label kind="synchronisation" x="467" y="433">pickup_success?</label>
<nail x="603" y="467"/>
<nail x="663" y="-59"/>
</transition>
<transition>
- <source ref="id14"/>
- <target ref="id12"/>
- <label kind="synchronisation" x="255" y="620">place_success?</label>
- <label kind="assignment" x="255" y="637">place_glass_retry--</label>
- <nail x="238" y="620"/>
+ <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>
</transition>
<transition>
- <source ref="id14"/>
- <target ref="id14"/>
+ <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>
@@ -174,34 +323,34 @@ chan place_fail, place_success;</declaration>
<nail x="867" y="586"/>
</transition>
<transition>
- <source ref="id15"/>
- <target ref="id14"/>
- <label kind="synchronisation" x="960" y="569">move_success?</label>
- <label kind="assignment" x="918" y="586">retry_count = init_retry_count</label>
+ <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>
</transition>
<transition>
- <source ref="id14"/>
- <target ref="id14"/>
+ <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"/>
</transition>
<transition>
- <source ref="id14"/>
- <target ref="id14"/>
+ <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"/>
</transition>
<transition>
- <source ref="id13"/>
- <target ref="id15"/>
+ <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>
</transition>
<transition>
- <source ref="id13"/>
- <target ref="id13"/>
+ <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>
@@ -209,73 +358,72 @@ chan place_fail, place_success;</declaration>
<nail x="994" y="510"/>
</transition>
<transition>
- <source ref="id13"/>
- <target ref="id13"/>
+ <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"/>
</transition>
<transition>
- <source ref="id13"/>
- <target ref="id13"/>
+ <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"/>
</transition>
<transition>
- <source ref="id12"/>
- <target ref="id13"/>
- <label kind="guard" x="263" y="476">place_glass_retry > 0</label>
- <label kind="synchronisation" x="263" y="459">pickup_success?</label>
- <label kind="assignment" x="263" y="493">retry_count = init_retry_count</label>
- <nail x="493" y="510"/>
+ <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>
</transition>
<transition>
- <source ref="id12"/>
- <target ref="id9"/>
+ <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"/>
</transition>
<transition>
- <source ref="id12"/>
- <target ref="id12"/>
- <label kind="guard" x="-85" y="433">retry_count > 1</label>
- <label kind="synchronisation" x="-85" y="450">pickup_fail?</label>
- <label kind="assignment" x="-85" y="467">retry_count--</label>
- <nail x="-103" y="518"/>
- <nail x="-10" y="543"/>
+ <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"/>
</transition>
<transition>
- <source ref="id12"/>
- <target ref="id12"/>
- <label kind="synchronisation" x="-102" y="340">move_fail?</label>
- <nail x="-68" y="340"/>
- <nail x="-76" y="357"/>
+ <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"/>
</transition>
<transition>
- <source ref="id12"/>
- <target ref="id12"/>
- <label kind="synchronisation" x="1" y="293">move_success?</label>
- <nail x="51" y="306"/>
- <nail x="-17" y="314"/>
+ <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"/>
</transition>
<transition>
- <source ref="id11"/>
- <target ref="id12"/>
- <label kind="synchronisation" x="212" y="306">place_success?</label>
- <label kind="assignment" x="51" y="323">retry_count = init_retry_count, place_glass_retry = init_retry_count</label>
+ <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>
</transition>
<transition>
- <source ref="id11"/>
- <target ref="id9"/>
+ <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"/>
</transition>
<transition>
- <source ref="id11"/>
- <target ref="id11"/>
+ <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>
@@ -283,50 +431,50 @@ chan place_fail, place_success;</declaration>
<nail x="552" y="204"/>
</transition>
<transition>
- <source ref="id11"/>
- <target ref="id11"/>
- <label kind="synchronisation" x="460" y="306">move_success?</label>
+ <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"/>
</transition>
<transition>
- <source ref="id11"/>
- <target ref="id11"/>
+ <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"/>
</transition>
<transition>
- <source ref="id6"/>
- <target ref="id6"/>
+ <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"/>
</transition>
<transition>
- <source ref="id6"/>
- <target ref="id6"/>
- <label kind="synchronisation" x="51" y="85">move_success?</label>
+ <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="id10"/>
- <target ref="id11"/>
+ <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="id10"/>
- <target ref="id9"/>
+ <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"/>
</transition>
<transition>
- <source ref="id10"/>
- <target ref="id10"/>
+ <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>
@@ -334,44 +482,44 @@ chan place_fail, place_success;</declaration>
<nail x="1037" y="382"/>
</transition>
<transition>
- <source ref="id8"/>
- <target ref="id10"/>
+ <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>
</transition>
<transition>
- <source ref="id8"/>
- <target ref="id8"/>
+ <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"/>
</transition>
<transition>
- <source ref="id7"/>
- <target ref="id8"/>
- <label kind="synchronisation" x="714" y="25">move_success?</label>
+ <source ref="id15"/>
+ <target ref="id28"/>
+ <label kind="synchronisation" x="697" y="25">move_success?</label>
</transition>
<transition>
- <source ref="id7"/>
- <target ref="id7"/>
- <label kind="synchronisation" x="544" y="-51">move_fail?</label>
+ <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"/>
</transition>
<transition>
- <source ref="id6"/>
- <target ref="id9"/>
+ <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>
</transition>
<transition>
- <source ref="id6"/>
- <target ref="id7"/>
- <label kind="synchronisation" x="365" y="25">pickup_success?</label>
+ <source ref="id14"/>
+ <target ref="id27"/>
+ <label kind="synchronisation" x="246" y="34">pickup_success?</label>
</transition>
<transition>
- <source ref="id6"/>
- <target ref="id6"/>
+ <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>
@@ -379,16 +527,55 @@ chan place_fail, place_success;</declaration>
<nail x="289" y="-42"/>
</transition>
<transition>
- <source ref="id5"/>
- <target ref="id6"/>
- <label kind="select" x="-102" y="144">i : int[1,5]</label>
- <label kind="synchronisation" x="-102" y="161">pressed?</label>
- <label kind="assignment" x="-102" y="178">init_retry_count = i, retry_count = init_retry_count</label>
+ <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>
+ </transition>
+ </template>
+ <template>
+ <name>positionupdatecatcher</name>
+ <location id="id33" x="-586" y="-170">
+ </location>
+ <init ref="id33"/>
+ <transition>
+ <source ref="id33"/>
+ <target ref="id33"/>
+ <label kind="synchronisation" x="-314" y="-195">position_update?</label>
+ <nail x="-416" y="-272"/>
+ <nail x="-391" y="-127"/>
+ </transition>
+ </template>
+ <template>
+ <name>positionupdate</name>
+ <location id="id34" x="0" y="0">
+ </location>
+ <init ref="id34"/>
+ <transition>
+ <source ref="id34"/>
+ <target ref="id34"/>
+ <label kind="synchronisation" x="34" y="-76">position_update!</label>
+ <nail x="-25" y="-119"/>
+ <nail x="178" y="-17"/>
+ </transition>
+ </template>
+ <template>
+ <name>movegoalcatcher</name>
+ <location id="id35" x="-603" y="-229">
+ </location>
+ <init ref="id35"/>
+ <transition>
+ <source ref="id35"/>
+ <target ref="id35"/>
+ <label kind="synchronisation" x="-450" y="-238">move_goal?</label>
+ <nail x="-442" y="-323"/>
+ <nail x="-450" y="-119"/>
</transition>
</template>
<system>// Place template instantiations here.
// List one or more processes to be composed into a system.
-system Cobot, press, move, pickup, place;
+system Cobot, press, move, pickup, place, movegoal, movegoalcatcher, positionupdate, positionupdatecatcher, model_positions;
</system>
<queries>
<query>
diff --git a/src/tron_adapter.cpp b/src/tron_adapter.cpp
index dc5ef9c..dd67bae 100644
--- a/src/tron_adapter.cpp
+++ b/src/tron_adapter.cpp
@@ -10,8 +10,12 @@
#include <moveit_msgs/PickupAction.h>
#include <moveit_msgs/PlaceAction.h>
#include <moveit_msgs/MoveItErrorCodes.h>
-#include <moveit_msgs/MoveGroupActionResult.h>
+#include <moveit_msgs/MoveGroupAction.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
+#include <gazebo_msgs/LinkStates.h>
+#include <geometry_msgs/Pose.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Vector3.h>
const std::string IP = "127.0.0.1";
const uint16_t PORT = 8080;
@@ -395,8 +399,7 @@ void mappings_callback_to_TRON(const ros::MessageEvent<T>& event){
}
-int32_t retry_count;
-
+int32_t retry_count; // set in configuration_phase
void on_pressure(const cooperation_msgs::PressureMsg::ConstPtr &msg) {
if (msg->pressed)
report_now("pressed", 1, &retry_count);
@@ -420,11 +423,87 @@ void on_move_result(const control_msgs::FollowJointTrajectoryActionResult::Const
else report_now("move_fail");
}
+void on_move_goal(const moveit_msgs::MoveGroupActionGoal::ConstPtr &msg){
+ int32_t is_orientation_constraint_set;
+ if (msg->goal.request.path_constraints.orientation_constraints.empty())
+ is_orientation_constraint_set = 0;
+ else is_orientation_constraint_set = 1;
+ report_now("move_goal", 1, &is_orientation_constraint_set);
+}
+
+bool compare_poses(geometry_msgs::Pose p1, geometry_msgs::Pose p2, double pos_tolerance = 0.01, double angle_tol = 0.1){
+ // q1 is inverse orientation from p1 (see w)
+ tf2::Quaternion q1(p1.orientation.x, p1.orientation.y, p1.orientation.z, -p1.orientation.w);
+
+ tf2::Quaternion q2(p2.orientation.x, p2.orientation.y, p2.orientation.z, p2.orientation.w);
+
+ tf2::Quaternion q_diff = q2 * q1; // order is important with quaternions
+
+ tf2Scalar angle_diff = q_diff.getAngleShortestPath();
+
+ double x_d = p1.position.x - p2.position.x;
+ double y_d = p1.position.y - p2.position.y;
+ double z_d = p1.position.z - p2.position.z;
+ double pos_diff = std::sqrt(x_d*x_d + y_d*y_d + z_d*z_d);
+ if (angle_diff < angle_tol * M_PI && pos_diff < pos_tolerance)
+ return true;
+ return false;
+}
+const std::vector<std::string> tron_vars_ordered = {
+ "bottle_in_start_pos",
+ "glass_in_start_pos",
+ "glass_in_target_pos"
+};
+std::vector<std::string> model_names = { "bottle::link", "glass::link", "glass::link"};
+std::vector<geometry_msgs::Pose> pose_to_compare_against; // set in configuration_phase
+std::vector<bool> init_states = {1,1,0};
+bool gazebo_initialized = false;
+void on_gazebo_link_states(const gazebo_msgs::LinkStates::ConstPtr &msg){
+ int32_t var[tron_vars_ordered.size()];
+ for (int i = 0; i < tron_vars_ordered.size(); i++) {
+ for (int j = 0; j < msg->name.size(); j++) {
+ if (msg->name[j] == model_names[i]) {
+ if (compare_poses(msg->pose[j], pose_to_compare_against[i]))
+ var[i] = 1;
+ else var[i] = 0;
+ break;
+ }
+ }
+ }
+ if (gazebo_initialized)
+ report_now("position_update", static_cast<uint16_t>(tron_vars_ordered.size()), var);
+ else {
+ for (int i = 0; i < tron_vars_ordered.size(); i++)
+ if (var[i] != init_states[i]) return;
+ gazebo_initialized = true;
+ }
+}
+
void configuration_phase(ros::NodeHandle& nh){
/* note: for configuration phase maximum message length is 256 Bytes,
therefore heap allocation can be avoided most of the time in called functions */
-
+
nh.getParam("/planning/retries", retry_count);
+ geometry_msgs::Pose pose;
+ nh.getParam("/object/bottle/x", pose.position.x);
+ nh.getParam("/object/bottle/y", pose.position.y);
+ nh.getParam("/object/bottle/z", pose.position.z);
+ pose.orientation.w = 1.0;
+ pose_to_compare_against.push_back(pose);
+
+ nh.getParam("/object/glass/x", pose.position.x);
+ nh.getParam("/object/glass/y", pose.position.y);
+ nh.getParam("/object/glass/z", pose.position.z);
+ pose.orientation.w = 1.0;
+ pose_to_compare_against.push_back(pose);
+
+ nh.getParam("/object/glass/place/x", pose.position.x);
+ nh.getParam("/object/glass/place/y", pose.position.y);
+ nh.getParam("/object/glass/place/z", pose.position.z);
+ pose.orientation.w = 1.0;
+ pose_to_compare_against.push_back(pose);
+
+
// note: since we are not an actual client (or server)
// we need to use the high level packet::*Action* messages.
// custom callbacks are implemented in order to not worry about header size
@@ -452,7 +531,17 @@ 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));
- // TODO: place and variables
+ map = Mapping("/move_group/goal", "move_goal", false);
+ map.add_var_to_mapping("orient_constraint_set");
+ mappings.push_back(map);
+ output_subscribers.push_back(nh.subscribe("/move_group/goal", 10, on_move_goal));
+
+ map = Mapping("/gazebo/link_states", "position_update", false);
+ map.add_var_to_mapping("bottle_in_start_pos");
+ map.add_var_to_mapping("glass_in_start_pos");
+ map.add_var_to_mapping("glass_in_target_pos");
+ mappings.push_back(map);
+ output_subscribers.push_back(nh.subscribe("/gazebo/link_states", 1, on_gazebo_link_states));
// not obvious in documentation: local variables are not supported
// add_var_to_channel(socketfd, "ausloesen", "lokal");
@@ -542,7 +631,7 @@ int main(int argc, char**argv){
request_start();
// testing phase loop
- ros::Rate test_phase_freq(60);
+ ros::Rate test_phase_freq(10);
while (ros::ok()) {
ros::spinOnce();
process_TRONs_msgs();
--
GitLab