From 24ae048269649e38ab4421a363585104df1282b3 Mon Sep 17 00:00:00 2001
From: CS <christoph.schroeter1@mailbox.tu-dresden.de>
Date: Mon, 9 Aug 2021 23:35:00 +0200
Subject: [PATCH]

---
 cobot1.xml                     | 172 +++++++++++++++++++++------------
 launch/cobot1_mock_test.launch |   5 +-
 scripts/tron_testing_auto.sh   |  49 ++++++++++
 src/tron_adapter.cpp           |  25 +++--
 4 files changed, 181 insertions(+), 70 deletions(-)
 create mode 100755 scripts/tron_testing_auto.sh

diff --git a/cobot1.xml b/cobot1.xml
index 6dd709d..6eedb6d 100644
--- a/cobot1.xml
+++ b/cobot1.xml
@@ -12,6 +12,7 @@ chan place_fail, place_success;
 chan move_fail, move_success;
 chan goal;
 chan position_update;
+chan test_done;
 int bottle_in_start_pos = 1;
 int glass_in_start_pos = 1;
 int glass_in_target_pos = 0;</declaration>
@@ -52,23 +53,29 @@ int glass_in_target_pos = 0;</declaration>
 		<location id="id5" x="-1079" y="-187">
 			<name x="-1173" y="-170">bottle_and_glass_end_pos</name>
 		</location>
-		<location id="id6" x="-986" y="8">
+		<location id="id6" x="-850" y="17">
 		</location>
-		<location id="id7" x="-986" y="-323">
-			<name x="-969" y="-340">glass_back_to_start</name>
+		<location id="id7" x="-850" y="-323">
 		</location>
 		<init ref="id1"/>
+		<transition>
+			<source ref="id7"/>
+			<target ref="id5"/>
+			<label kind="synchronisation" x="-1062" y="-357">position_update?</label>
+			<label kind="assignment" x="-1062" y="-340">glass_in_target_pos = 1</label>
+			<nail x="-1079" y="-323"/>
+		</transition>
 		<transition>
 			<source ref="id4"/>
 			<target ref="id6"/>
-			<label kind="synchronisation" x="-1283" y="-85">position_update?</label>
-			<label kind="assignment" x="-1283" y="-68">glass_in_start_pos = 1</label>
+			<label kind="synchronisation" x="-1275" y="-102">position_update?</label>
+			<label kind="assignment" x="-1283" y="-85">glass_in_start_pos = 1</label>
 		</transition>
 		<transition>
 			<source ref="id5"/>
 			<target ref="id7"/>
-			<label kind="synchronisation" x="-1147" y="-289">position_update?</label>
-			<label kind="assignment" x="-1198" y="-272">glass_in_target_pos = 0</label>
+			<label kind="synchronisation" x="-1003" y="-238">position_update?</label>
+			<label kind="assignment" x="-1037" y="-221">glass_in_target_pos = 0</label>
 		</transition>
 		<transition>
 			<source ref="id3"/>
@@ -87,8 +94,8 @@ int glass_in_target_pos = 0;</declaration>
 		<transition>
 			<source ref="id7"/>
 			<target ref="id6"/>
-			<label kind="synchronisation" x="-977" y="-272">position_update?</label>
-			<label kind="assignment" x="-977" y="-255">glass_in_start_pos = 1</label>
+			<label kind="synchronisation" x="-841" y="-212">position_update?</label>
+			<label kind="assignment" x="-841" y="-195">glass_in_start_pos = 1</label>
 		</transition>
 		<transition>
 			<source ref="id4"/>
@@ -187,10 +194,10 @@ int glass_in_target_pos = 0;</declaration>
 		<location id="id13" x="-221" y="-119">
 		</location>
 		<location id="id14" x="127" y="-119">
-			<name x="34" y="-153">pick_bottle</name>
+			<name x="17" y="-144">picking_bottle</name>
 		</location>
 		<location id="id15" x="578" y="-119">
-			<name x="535" y="-110">move_to_glass</name>
+			<name x="535" y="-110">moving_to_glass</name>
 			<label kind="invariant" x="467" y="-93">bottle_in_start_pos == false</label>
 		</location>
 		<location id="id16" x="773" y="-8">
@@ -203,40 +210,45 @@ int glass_in_target_pos = 0;</declaration>
 			<name x="688" y="102">stop_pour</name>
 		</location>
 		<location id="id19" x="357" y="76">
-			<name x="255" y="51">place_bottle</name>
+			<name x="246" y="51">placing_bottle</name>
 		</location>
 		<location id="id20" x="-246" y="76">
-			<name x="-365" y="34">pick_glass</name>
+			<name x="-374" y="34">picking_glass</name>
 			<label kind="invariant" x="-459" y="51">bottle_in_start_pos == true</label>
 		</location>
 		<location id="id21" x="314" y="365">
-			<name x="271" y="373">place_glass_target</name>
+			<name x="246" y="374">placing_glass_target</name>
 		</location>
 		<location id="id22" x="-246" y="612">
-			<name x="-238" y="570">place_glass_orig</name>
+			<name x="-238" y="570">placing_glass_start</name>
 		</location>
 		<location id="id23" x="314" y="552">
 			<name x="314" y="569">goto_last_state</name>
 		</location>
 		<location id="id24" x="1113" y="612">
-			<name x="952" y="620">goto_last_state_orig</name>
+			<name x="892" 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="501">
-			<name x="833" y="493">finished_success</name>
+		<location id="id26" x="977" y="527">
+			<name x="841" y="518">finished_success</name>
 		</location>
 		<location id="id27" x="365" y="-119">
+			<name x="323" y="-102">bottle_picked</name>
 		</location>
 		<location id="id28" x="773" y="-119">
+			<name x="773" y="-153">moved_to_glass</name>
 		</location>
 		<location id="id29" x="-59" y="365">
+			<name x="-127" y="374">glass_picked</name>
 		</location>
 		<location id="id30" x="671" y="365">
+			<name x="603" y="331">glass_placed_target</name>
 			<label kind="invariant" x="595" y="382">glass_in_target_pos == true</label>
 		</location>
 		<location id="id31" x="-246" y="408">
+			<name x="-391" y="399">glass_placed_start</name>
 		</location>
 		<location id="id32" x="314" y="467">
 			<name x="340" y="459">stepping_back</name>
@@ -249,20 +261,46 @@ int glass_in_target_pos = 0;</declaration>
 		<location id="id35" x="-127" y="-221">
 		</location>
 		<location id="id36" x="535" y="76">
+			<name x="535" y="85">pouring_done</name>
 		</location>
 		<location id="id37" x="-25" y="76">
+			<name x="-17" y="51">bottle_placed</name>
+		</location>
+		<location id="id38" x="977" y="459">
+			<name x="875" y="450">in_start_pos</name>
+		</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>
 		<init ref="id33"/>
+		<transition>
+			<source ref="id40"/>
+			<target ref="id17"/>
+			<label kind="synchronisation" x="1028" y="-246">test_done!</label>
+			<nail x="1062" y="-263"/>
+		</transition>
+		<transition>
+			<source ref="id39"/>
+			<target ref="id24"/>
+			<label kind="synchronisation" x="807" y="595">move_success?</label>
+		</transition>
+		<transition>
+			<source ref="id38"/>
+			<target ref="id26"/>
+			<label kind="synchronisation" x="892" y="484">test_done!</label>
+		</transition>
 		<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>
+			<label kind="assignment" x="-229" 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="synchronisation" x="433" y="34">goal?</label>
 			<label kind="assignment" x="382" y="51">orient_constraint_set = true</label>
 		</transition>
 		<transition>
@@ -287,7 +325,7 @@ int glass_in_target_pos = 0;</declaration>
 			<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>
+			<label kind="assignment" x="-442" y="-246">init_retry_count = i, retry_count = init_retry_count</label>
 			<nail x="-374" y="-221"/>
 		</transition>
 		<transition>
@@ -303,8 +341,8 @@ int glass_in_target_pos = 0;</declaration>
 		<transition>
 			<source ref="id31"/>
 			<target ref="id20"/>
-			<label kind="synchronisation" x="-357" y="298">goal?</label>
-			<label kind="assignment" x="-450" y="315">orient_constraint_set = false</label>
+			<label kind="synchronisation" x="-357" y="289">goal?</label>
+			<label kind="assignment" x="-450" y="306">orient_constraint_set = false</label>
 		</transition>
 		<transition>
 			<source ref="id30"/>
@@ -332,8 +370,9 @@ int glass_in_target_pos = 0;</declaration>
 		</transition>
 		<transition>
 			<source ref="id25"/>
-			<target ref="id26"/>
-			<label kind="synchronisation" x="858" y="450">move_success?</label>
+			<target ref="id38"/>
+			<label kind="synchronisation" x="867" y="408">move_success?</label>
+			<nail x="977" y="416"/>
 		</transition>
 		<transition>
 			<source ref="id21"/>
@@ -342,24 +381,24 @@ int glass_in_target_pos = 0;</declaration>
 		</transition>
 		<transition>
 			<source ref="id24"/>
-			<target ref="id17"/>
+			<target ref="id40"/>
 			<label kind="synchronisation" x="1003" y="561">move_success?</label>
-			<nail x="1113" y="-297"/>
+			<nail x="1113" y="-187"/>
 		</transition>
 		<transition>
 			<source ref="id22"/>
-			<target ref="id24"/>
-			<label kind="guard" x="629" y="578">retry_count == 1</label>
-			<label kind="synchronisation" x="629" y="595">place_fail?</label>
+			<target ref="id39"/>
+			<label kind="guard" x="493" y="569">retry_count == 1</label>
+			<label kind="synchronisation" x="493" y="586">place_fail?</label>
 		</transition>
 		<transition>
 			<source ref="id20"/>
-			<target ref="id17"/>
+			<target ref="id40"/>
 			<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"/>
+			<nail x="1079" y="-153"/>
 		</transition>
 		<transition>
 			<source ref="id22"/>
@@ -392,7 +431,7 @@ int glass_in_target_pos = 0;</declaration>
 		<transition>
 			<source ref="id22"/>
 			<target ref="id22"/>
-			<label kind="synchronisation" x="-400" y="664">move_success?</label>
+			<label kind="synchronisation" x="-399" y="654">move_success?</label>
 			<nail x="-264" y="672"/>
 			<nail x="-297" y="655"/>
 		</transition>
@@ -434,21 +473,21 @@ int glass_in_target_pos = 0;</declaration>
 		</transition>
 		<transition>
 			<source ref="id20"/>
-			<target ref="id17"/>
+			<target ref="id40"/>
 			<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"/>
+			<nail x="1096" y="-170"/>
 		</transition>
 		<transition>
 			<source ref="id20"/>
 			<target ref="id20"/>
-			<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"/>
+			<label kind="guard" x="-425" y="110">retry_count &gt; 1</label>
+			<label kind="synchronisation" x="-399" y="127">pickup_fail?</label>
+			<label kind="assignment" x="-399" y="144">retry_count--</label>
+			<nail x="-314" y="119"/>
+			<nail x="-297" y="136"/>
 		</transition>
 		<transition>
 			<source ref="id20"/>
@@ -472,11 +511,10 @@ int glass_in_target_pos = 0;</declaration>
 		</transition>
 		<transition>
 			<source ref="id19"/>
-			<target ref="id17"/>
+			<target ref="id40"/>
 			<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"/>
@@ -492,7 +530,7 @@ int glass_in_target_pos = 0;</declaration>
 			<target ref="id19"/>
 			<label kind="synchronisation" x="374" y="136">move_success?</label>
 			<nail x="383" y="136"/>
-			<nail x="434" y="110"/>
+			<nail x="416" y="110"/>
 		</transition>
 		<transition>
 			<source ref="id19"/>
@@ -548,10 +586,11 @@ int glass_in_target_pos = 0;</declaration>
 		</transition>
 		<transition>
 			<source ref="id14"/>
-			<target ref="id17"/>
-			<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"/>
+			<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"/>
 		</transition>
 		<transition>
 			<source ref="id14"/>
@@ -575,14 +614,27 @@ int glass_in_target_pos = 0;</declaration>
 			<label kind="assignment" x="-280" y="-85">init_retry_count = i, retry_count = init_retry_count</label>
 		</transition>
 	</template>
+	<template>
+		<name>testdonecatcher</name>
+		<location id="id41" x="0" y="0">
+		</location>
+		<init ref="id41"/>
+		<transition>
+			<source ref="id41"/>
+			<target ref="id41"/>
+			<label kind="synchronisation" x="8" y="-42">test_done?</label>
+			<nail x="59" y="-17"/>
+			<nail x="59" y="17"/>
+		</transition>
+	</template>
 	<template>
 		<name>positionupdatecatcher</name>
-		<location id="id38" x="-544" y="-331">
+		<location id="id42" x="-544" y="-331">
 		</location>
-		<init ref="id38"/>
+		<init ref="id42"/>
 		<transition>
-			<source ref="id38"/>
-			<target ref="id38"/>
+			<source ref="id42"/>
+			<target ref="id42"/>
 			<label kind="synchronisation" x="-493" y="-340">position_update?</label>
 			<nail x="-501" y="-357"/>
 			<nail x="-501" y="-306"/>
@@ -590,12 +642,12 @@ int glass_in_target_pos = 0;</declaration>
 	</template>
 	<template>
 		<name>positionupdate</name>
-		<location id="id39" x="0" y="0">
+		<location id="id43" x="0" y="0">
 		</location>
-		<init ref="id39"/>
+		<init ref="id43"/>
 		<transition>
-			<source ref="id39"/>
-			<target ref="id39"/>
+			<source ref="id43"/>
+			<target ref="id43"/>
 			<label kind="synchronisation" x="59" y="-8">position_update!</label>
 			<nail x="51" y="-17"/>
 			<nail x="51" y="17"/>
@@ -603,12 +655,12 @@ int glass_in_target_pos = 0;</declaration>
 	</template>
 	<template>
 		<name>goalcatcher</name>
-		<location id="id40" x="-578" y="-263">
+		<location id="id44" x="-578" y="-263">
 		</location>
-		<init ref="id40"/>
+		<init ref="id44"/>
 		<transition>
-			<source ref="id40"/>
-			<target ref="id40"/>
+			<source ref="id44"/>
+			<target ref="id44"/>
 			<label kind="synchronisation" x="-535" y="-272">goal?</label>
 			<nail x="-535" y="-280"/>
 			<nail x="-535" y="-246"/>
@@ -616,7 +668,7 @@ int glass_in_target_pos = 0;</declaration>
 	</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;
+system Cobot, press, move, pickup, place, goal_, goalcatcher, positionupdate, positionupdatecatcher, model_positions, testdonecatcher;
     </system>
 	<queries>
 		<query>
diff --git a/launch/cobot1_mock_test.launch b/launch/cobot1_mock_test.launch
index 30caaf1..09a9611 100644
--- a/launch/cobot1_mock_test.launch
+++ b/launch/cobot1_mock_test.launch
@@ -1,6 +1,8 @@
 <launch>
 
-    <include file="$(find panda_simulation)/launch/simulation.launch"/>
+    <include file="$(find panda_simulation)/launch/simulation.launch">
+        <arg name="gui" value="false"/>
+    </include>
 
     <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
 
@@ -20,6 +22,7 @@
     <!-- The execution_duration_monitoring sometimes aborts a valid trajectory execution -->
     <param name="move_group/trajectory_execution/execution_duration_monitoring" value="false" />
     
+    <!-- setting higher values in this script can cause more failures -->
     <node pkg="cobot_1" type="gazebo_real_time.sh" name="gazebo_real_time" output="screen"/>
 
 </launch>
diff --git a/scripts/tron_testing_auto.sh b/scripts/tron_testing_auto.sh
new file mode 100755
index 0000000..ef67fd2
--- /dev/null
+++ b/scripts/tron_testing_auto.sh
@@ -0,0 +1,49 @@
+#!/bin/bash
+
+start_dir=$PWD
+test_count=10
+ros_workspace_location="/home/cs/Schreibtisch/panda_gazebo_workspace"
+package_name="cobot_1"
+roslaunch_filename="cobot1_mock_test.launch"
+tron_location="/home/cs/Schreibtisch/TRON/tron"
+model_location="/home/cs/Schreibtisch/TRON/cobot1.xml"
+
+# make sure package was built and devel/setup.bash was run before starting
+cd $ros_workspace_location
+catkin build $package_name
+source devel/setup.bash
+
+tron_args=( # do not set -o here
+    "-I SocketAdapter" 
+    "-v 26"
+    "-q"
+    "-P eager"
+    $model_location # needs to be specified before port
+    "-- 8080"
+)
+
+
+
+# tron_args to one string
+tron_args_str=${tron_args[0]}
+for single_arg in ${tron_args[@]:1}; do
+    tron_args_str+=" "
+    tron_args_str+=$single_arg
+done
+
+echo "args given to TRON: "
+echo $tron_args_str
+
+for ((i=1;i<=test_count;i++)); do 
+    tron_args_str_output="-o $start_dir/test_output$i $tron_args_str"
+    $tron_location $tron_args_str_output & tron_pid=$!
+    roslaunch $package_name $roslaunch_filename & roslaunch_pid=$!
+    trap "kill $tron_pid; kill $roslaunch_pid; wait $tron_pid; wait $roslaunch_pid; return 1" SIGINT # kill both on interrupt
+    wait $tron_pid
+    kill $roslaunch_pid
+    wait $roslaunch_pid # wait till actually killed, takes few seconds
+    echo "test $i done"
+done
+
+cd $start_dir
+return 0
\ No newline at end of file
diff --git a/src/tron_adapter.cpp b/src/tron_adapter.cpp
index 4d0db18..5c385c7 100644
--- a/src/tron_adapter.cpp
+++ b/src/tron_adapter.cpp
@@ -503,6 +503,11 @@ void on_gazebo_link_states(const gazebo_msgs::LinkStates::ConstPtr &msg){
     }
 }
 
+//intentionally send not expected output to end TRON testing
+void on_test_done(Mapping& map, int32_t *ptr){
+    report_now("move_success");
+}
+
 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 */
@@ -577,6 +582,11 @@ void configuration_phase(ros::NodeHandle& nh){
     mappings.push_back(map);
     output_subscribers.push_back(nh.subscribe("/gazebo/link_states", 1, on_gazebo_link_states));
 
+    // used to end testing when model is done
+    map = Mapping("test_done_dummy", "test_done", true);
+    map.input_callback = on_test_done;
+    mappings.push_back(map);
+
     // not obvious in documentation: local variables are not supported
     // add_var_to_channel(socketfd, "ausloesen", "lokal");
     uint64_t microseconds = 1000000; // one second
@@ -636,15 +646,12 @@ void process_TRONs_msgs(){
         }
 
         for (Mapping& map : mappings)
-            if (map.channel.name == chan->name && map.channel.is_input)
-                for (ros::Publisher& pub : input_publishers){
-                    if (pub.getTopic() == map.topic) {
-                        if (map.input_callback != nullptr)
-                            map.input_callback(map, vals);  
-                        else throw "no callback declared";  
-                        break;
-                    }
-                }
+            if (map.channel.name == chan->name && map.channel.is_input) {
+                if (map.input_callback != nullptr)
+                    map.input_callback(map, vals);  
+                else throw "no callback declared";
+            }
+                          
 
         // send acknowledgement
         add_int32_in_network_order(ACK_SINGLE, bytes, 0);
-- 
GitLab