diff --git a/cobot1.xml b/cobot1.xml
index 6eedb6d43a7e6ea010c7c0db85eb28fc1dc4a5cb..364a50acf5ce97769e234d344cd9815a9c321080 100644
--- a/cobot1.xml
+++ b/cobot1.xml
@@ -13,9 +13,32 @@ 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>
+chan intentional_fail;
+
+const int max_pos_diff = 1000;
+const int max_ang_diff = 180;
+int bottle_diff_to_start_pos;
+int bottle_diff_to_start_ang;
+int bottle_diff_to_target_pos;
+int bottle_diff_to_target_ang;
+int glass_diff_to_start_pos;
+int glass_diff_to_start_ang;
+int glass_diff_to_target_pos;
+int glass_diff_to_target_ang;
+chan bottle_diff_to_start_pos_update;
+chan bottle_diff_to_start_ang_update;
+chan bottle_diff_to_target_pos_update;
+chan bottle_diff_to_target_ang_update;
+chan glass_diff_to_start_pos_update;
+chan glass_diff_to_start_ang_update;
+chan glass_diff_to_target_pos_update;
+chan glass_diff_to_target_ang_update;
+
+bool bottle_in_start_pos = true;
+bool bottle_in_target_pos = false;
+bool glass_in_start_pos = true;
+bool glass_in_target_pos = false;
+</declaration>
<template>
<name>place</name>
<location id="id0" x="0" y="0">
@@ -38,88 +61,96 @@ int glass_in_target_pos = 0;</declaration>
</template>
<template>
<name>model_positions</name>
- <location id="id1" x="-1547" y="-416">
- <name x="-1598" y="-445">start_pos</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="-1343" y="-416">
- <name x="-1326" y="-425">bottle_moving</name>
+ <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="-1343" y="-297">
- <name x="-1326" y="-323">bottle_end_pos</name>
+ <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="-1343" y="-187">
- <name x="-1454" y="-178">glass_moving</name>
+ <location id="id4" x="-1487" y="-824">
+ <name x="-1598" y="-815">glass_moving</name>
</location>
- <location id="id5" x="-1079" y="-187">
- <name x="-1173" y="-170">bottle_and_glass_end_pos</name>
+ <location id="id5" x="-1223" y="-824">
+ <name x="-1317" y="-807">bottle_and_glass_end_pos</name>
</location>
- <location id="id6" x="-850" y="17">
+ <location id="id6" x="-918" y="-620">
+ <name x="-994" y="-603">glass_start_pos_again</name>
</location>
- <location id="id7" x="-850" y="-323">
+ <location id="id7" x="-918" y="-1062">
+ <name x="-952" y="-1096">glass_picked_up_again</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="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"/>
+ <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"/>
</transition>
<transition>
<source ref="id4"/>
<target ref="id6"/>
- <label kind="synchronisation" x="-1275" y="-102">position_update?</label>
- <label kind="assignment" x="-1283" y="-85">glass_in_start_pos = 1</label>
+ <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>
</transition>
<transition>
<source ref="id5"/>
<target ref="id7"/>
- <label kind="synchronisation" x="-1003" y="-238">position_update?</label>
- <label kind="assignment" x="-1037" y="-221">glass_in_target_pos = 0</label>
+ <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>
</transition>
<transition>
<source ref="id3"/>
<target ref="id2"/>
- <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"/>
+ <label kind="assignment" x="-1717" y="-960">bottle_in_target_pos = false</label>
+ <nail x="-1555" y="-985"/>
</transition>
<transition>
<source ref="id6"/>
<target ref="id4"/>
- <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"/>
+ <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"/>
</transition>
<transition>
<source ref="id7"/>
<target ref="id6"/>
- <label kind="synchronisation" x="-841" y="-212">position_update?</label>
- <label kind="assignment" x="-841" y="-195">glass_in_start_pos = 1</label>
+ <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>
</transition>
<transition>
<source ref="id4"/>
<target ref="id5"/>
- <label kind="synchronisation" x="-1309" y="-221">position_update?</label>
- <label kind="assignment" x="-1309" y="-204">glass_in_target_pos = 1</label>
+ <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>
</transition>
<transition>
<source ref="id3"/>
<target ref="id4"/>
- <label kind="synchronisation" x="-1504" y="-263">position_update?</label>
- <label kind="assignment" x="-1504" y="-246">glass_in_start_pos = 0</label>
+ <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="synchronisation" x="-1334" y="-391">position_update?</label>
- <label kind="assignment" x="-1334" y="-374">bottle_in_start_pos = 1</label>
+ <label kind="assignment" x="-1479" y="-1003">bottle_in_target_pos = true</label>
</transition>
<transition>
<source ref="id1"/>
<target ref="id2"/>
- <label kind="synchronisation" x="-1479" y="-459">position_update?</label>
- <label kind="assignment" x="-1513" y="-442">bottle_in_start_pos = 0</label>
+ <label kind="assignment" x="-1683" y="-1079">bottle_in_start_pos = false</label>
</transition>
</template>
<template>
@@ -201,20 +232,20 @@ int glass_in_target_pos = 0;</declaration>
<label kind="invariant" x="467" y="-93">bottle_in_start_pos == false</label>
</location>
<location id="id16" x="773" y="-8">
- <name x="679" y="1">start_pour</name>
+ <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>
- <location id="id18" x="773" y="110">
- <name x="688" y="102">stop_pour</name>
+ <location id="id18" x="773" y="76">
+ <name x="748" y="85">stop_pour</name>
</location>
<location id="id19" x="357" y="76">
<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="-459" y="51">bottle_in_start_pos == true</label>
+ <label kind="invariant" x="-467" y="51">bottle_in_target_pos == true</label>
</location>
<location id="id21" x="314" y="365">
<name x="246" y="374">placing_glass_target</name>
@@ -238,14 +269,15 @@ int glass_in_target_pos = 0;</declaration>
<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>
+ <name x="790" y="-136">moved_to_glass</name>
</location>
<location id="id29" x="-59" y="365">
<name x="-127" y="374">glass_picked</name>
+ <label kind="invariant" x="-144" y="391">glass_in_start_pos == false</label>
</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>
+ <label kind="invariant" x="586" 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>
@@ -267,7 +299,8 @@ int glass_in_target_pos = 0;</declaration>
<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>
+ <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>
<location id="id39" x="663" y="612">
<name x="578" y="629">steping_back_start</name>
@@ -310,22 +343,15 @@ int glass_in_target_pos = 0;</declaration>
<transition>
<source ref="id35"/>
<target ref="id34"/>
- <label kind="synchronisation" x="-136" y="-195">move_success?</label>
+ <label kind="synchronisation" x="-59" y="-246">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="-442" y="-246">init_retry_count = i, retry_count = init_retry_count</label>
+ <label kind="select" x="-357" y="-263">i : int[1,15]</label>
+ <label kind="synchronisation" x="-357" y="-280">pressed?</label>
+ <label kind="assignment" x="-442" y="-246">init_retry_count = i, retry_count = i</label>
<nail x="-374" y="-221"/>
</transition>
<transition>
@@ -483,9 +509,9 @@ int glass_in_target_pos = 0;</declaration>
<transition>
<source ref="id20"/>
<target ref="id20"/>
- <label kind="guard" x="-425" y="110">retry_count > 1</label>
- <label kind="synchronisation" x="-399" y="127">pickup_fail?</label>
- <label kind="assignment" x="-399" y="144">retry_count--</label>
+ <label kind="guard" x="-425" y="119">retry_count > 1</label>
+ <label kind="synchronisation" x="-399" y="136">pickup_fail?</label>
+ <label kind="assignment" x="-399" y="153">retry_count--</label>
<nail x="-314" y="119"/>
<nail x="-297" y="136"/>
</transition>
@@ -535,7 +561,7 @@ int glass_in_target_pos = 0;</declaration>
<transition>
<source ref="id19"/>
<target ref="id19"/>
- <label kind="synchronisation" x="264" y="119">move_fail?</label>
+ <label kind="synchronisation" x="263" y="127">move_fail?</label>
<nail x="298" y="110"/>
<nail x="349" y="136"/>
</transition>
@@ -563,27 +589,13 @@ int glass_in_target_pos = 0;</declaration>
<transition>
<source ref="id16"/>
<target ref="id18"/>
- <label kind="synchronisation" x="782" y="42">move_success?</label>
- </transition>
- <transition>
- <source ref="id16"/>
- <target ref="id16"/>
- <label kind="synchronisation" x="875" y="-17">move_fail?</label>
- <nail x="867" y="-42"/>
- <nail x="867" y="26"/>
+ <label kind="synchronisation" x="782" y="25">move_success?</label>
</transition>
<transition>
<source ref="id15"/>
<target ref="id28"/>
<label kind="synchronisation" x="620" y="-136">move_success?</label>
</transition>
- <transition>
- <source ref="id15"/>
- <target ref="id15"/>
- <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="id40"/>
@@ -609,9 +621,9 @@ int glass_in_target_pos = 0;</declaration>
<transition>
<source ref="id13"/>
<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>
+ <label kind="select" x="-170" y="-102">i : int[1,15]</label>
+ <label kind="synchronisation" x="-170" y="-119">pressed?</label>
+ <label kind="assignment" x="-246" y="-85">init_retry_count = i, retry_count = i</label>
</transition>
</template>
<template>
@@ -629,15 +641,80 @@ int glass_in_target_pos = 0;</declaration>
</template>
<template>
<name>positionupdatecatcher</name>
- <location id="id42" x="-544" y="-331">
+ <location id="id42" x="-731" y="-544">
</location>
<init ref="id42"/>
<transition>
<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"/>
+ <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>
+ <nail x="-977" y="-765"/>
+ <nail x="-935" y="-799"/>
+ </transition>
+ <transition>
+ <source ref="id42"/>
+ <target ref="id42"/>
+ <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>
+ <nail x="-433" y="-739"/>
+ <nail x="-484" y="-790"/>
+ </transition>
+ <transition>
+ <source ref="id42"/>
+ <target ref="id42"/>
+ <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>
+ <nail x="-476" y="-416"/>
+ <nail x="-510" y="-382"/>
+ </transition>
+ <transition>
+ <source ref="id42"/>
+ <target ref="id42"/>
+ <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>
+ <nail x="-986" y="-433"/>
+ <nail x="-952" y="-391"/>
+ </transition>
+ <transition>
+ <source ref="id42"/>
+ <target ref="id42"/>
+ <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>
+ <nail x="-782" y="-331"/>
+ <nail x="-697" y="-331"/>
+ </transition>
+ <transition>
+ <source ref="id42"/>
+ <target ref="id42"/>
+ <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>
+ <nail x="-1003" y="-578"/>
+ <nail x="-1003" y="-527"/>
+ </transition>
+ <transition>
+ <source ref="id42"/>
+ <target ref="id42"/>
+ <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>
+ <nail x="-663" y="-850"/>
+ <nail x="-765" y="-850"/>
+ </transition>
+ <transition>
+ <source ref="id42"/>
+ <target ref="id42"/>
+ <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>
+ <nail x="-331" y="-595"/>
+ <nail x="-331" y="-527"/>
</transition>
</template>
<template>
@@ -648,9 +725,58 @@ int glass_in_target_pos = 0;</declaration>
<transition>
<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"/>
+ <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"/>
+ <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"/>
+ <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"/>
+ <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"/>
+ <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"/>
+ <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"/>
+ <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"/>
+ <label kind="synchronisation" x="127" y="-8">bottle_diff_to_start_pos_update!</label>
+ <nail x="119" y="-17"/>
+ <nail x="119" y="17"/>
</transition>
</template>
<template>
diff --git a/launch/cobot1_mock_test.launch b/launch/cobot1_mock_test.launch
index 09a9611957527a04a5e698b2ecaccd58823eb548..2eaf8f0114bf57745c9793857e089b9dc7e198b4 100644
--- a/launch/cobot1_mock_test.launch
+++ b/launch/cobot1_mock_test.launch
@@ -1,16 +1,19 @@
<launch>
<include file="$(find panda_simulation)/launch/simulation.launch">
- <arg name="gui" value="false"/>
+ <arg name="gui" value="true"/>
+ <arg name="slow_simulation" value="true" />
</include>
+ <!--
<include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
+ -->
<include file="$(find safe_zone_controller)/launch/safe_zone_controller.launch" />
<include file="$(find pressure_sensor)/launch/pressure_sensor_mock.launch" />
- <node name="Cobot1_TRONAdapter" pkg="cobot_1" type="TRON_Adapter" respawn="false" output="screen"/>
+ <node name="Cobot1_TRONAdapter" pkg="cobot_1" type="TRON_Adapter" respawn="false" output="screen"/>
<node name="Cobot1_Controller" pkg="cobot_1" type="CobotController" respawn="false" output="screen">
<rosparam command="load" file="$(find cobot_1)/config/cobot1_config.yaml" />
@@ -21,8 +24,5 @@
<!-- 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
index ef67fd241cee7fe46b233bb279508f2d89f22553..8d21b445a54fcf5231db0a9d7c13db03694112e6 100755
--- a/scripts/tron_testing_auto.sh
+++ b/scripts/tron_testing_auto.sh
@@ -1,7 +1,7 @@
#!/bin/bash
start_dir=$PWD
-test_count=10
+test_count=50
ros_workspace_location="/home/cs/Schreibtisch/panda_gazebo_workspace"
package_name="cobot_1"
roslaunch_filename="cobot1_mock_test.launch"
@@ -34,7 +34,7 @@ done
echo "args given to TRON: "
echo $tron_args_str
-for ((i=1;i<=test_count;i++)); do
+for ((i=29;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=$!
@@ -46,4 +46,4 @@ for ((i=1;i<=test_count;i++)); do
done
cd $start_dir
-return 0
\ No newline at end of file
+return 0
diff --git a/src/tron_adapter.cpp b/src/tron_adapter.cpp
index 5c385c7202e873a713e26d315a74af4c66b61a58..a1b17404fc69a6ba90c2831fbca8e7aa4941fc16 100644
--- a/src/tron_adapter.cpp
+++ b/src/tron_adapter.cpp
@@ -14,9 +14,11 @@
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <gazebo_msgs/LinkStates.h>
#include <geometry_msgs/Pose.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Vector3.h>
#include <algorithm> // for std::find
+#include <cmath>
const std::string IP = "127.0.0.1";
const uint16_t PORT = 8080;
@@ -406,6 +408,8 @@ void mappings_callback_to_TRON(const ros::MessageEvent<T>& event){
}
}
+// ---------------------------------------------------------------------------------------------------------------------------------------
+// custom from here
int32_t retry_count; // set in configuration_phase
void on_pressure(const cooperation_msgs::PressureMsg::ConstPtr &msg) {
@@ -455,57 +459,79 @@ void on_place_goal(const moveit_msgs::PlaceActionGoal::ConstPtr &msg) {
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){
- // q1 is inverse orientation from p1 (see w)
- tf2::Quaternion q1(p1.orientation.x, p1.orientation.y, p1.orientation.z, -p1.orientation.w);
+double get_pos_diff(geometry_msgs::Point p1, geometry_msgs::Point p2) {
+ double x_d = p1.x - p2.x;
+ double y_d = p1.y - p2.y;
+ double z_d = p1.z - p2.z;
+ return std::sqrt(x_d*x_d + y_d*y_d + z_d*z_d);
+}
+// factor 100 means centimeters
+int32_t get_pos_diff_int32(geometry_msgs::Point p1, geometry_msgs::Point p2, int32_t factor = 100) {
+ 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;
+ tf2::fromMsg(q_msg1, q1);
+ tf2::fromMsg(q_msg2, q2);
+ return q1.angleShortestPath(q2);
+}
+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);
+}
- 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 && 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"
+const std::vector<std::string> tron_pos_vars_ordered = {
+ "bottle_diff_to_start_pos",
+ "bottle_diff_to_start_ang",
+ "bottle_diff_to_target_pos",
+ "bottle_diff_to_target_ang",
+ "glass_diff_to_start_pos",
+ "glass_diff_to_start_ang",
+ "glass_diff_to_target_pos",
+ "glass_diff_to_target_ang"
};
-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;
+ int bottle_index = -1;
+ int glass_index = -1;
+ for (int j = 0; j < msg->name.size(); j++) {
+ if (msg->name[j] == "bottle::link") bottle_index = j;
+ if (msg->name[j] == "glass::link") glass_index = j;
+ }
+ 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[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[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[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);
+ 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++) {
+ if (tron_pos_vars_ordered[i] == map.channel.vars[0]) {
+ if (gazebo_initialized) {
+ report_now(map.channel.name, 1, &vars[i]);
+ }
+ }
}
+
}
}
- 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;
+ if (!gazebo_initialized) {
+ int init_diff = 1; // position difference needed to assume gazebo is initialized
+ if (vars[0] > init_diff) return; // bottle init
+ if (vars[4] > init_diff) return; // glass init
gazebo_initialized = true;
}
}
//intentionally send not expected output to end TRON testing
void on_test_done(Mapping& map, int32_t *ptr){
- report_now("move_success");
+ report_now("intentional_fail");
}
void configuration_phase(ros::NodeHandle& nh){
@@ -513,19 +539,28 @@ void configuration_phase(ros::NodeHandle& nh){
therefore heap allocation can be avoided most of the time in called functions */
nh.getParam("/planning/retries", retry_count);
+
+
geometry_msgs::Pose pose;
+ // bottle start
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);
-
+ // bottle target
+ nh.getParam("/object/bottle/place/x", pose.position.x);
+ nh.getParam("/object/bottle/place/y", pose.position.y);
+ nh.getParam("/object/bottle/place/z", pose.position.z);
+ pose.orientation.w = 1.0;
+ pose_to_compare_against.push_back(pose);
+ // glass start
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);
-
+ // glass target
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);
@@ -575,17 +610,19 @@ void configuration_phase(ros::NodeHandle& nh){
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");
- map.add_var_to_mapping("glass_in_target_pos");
- mappings.push_back(map);
+ for (std::string s : tron_pos_vars_ordered) {
+ map = Mapping("/gazebo/link_states", s+"_update", false);
+ map.add_var_to_mapping(s);
+ 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);
+ map = Mapping("test_done_dummy", "intentional_fail", false);
+ mappings.push_back(map);
// not obvious in documentation: local variables are not supported
// add_var_to_channel(socketfd, "ausloesen", "lokal");
@@ -661,16 +698,15 @@ void process_TRONs_msgs(){
}
int main(int argc, char**argv){
- ros::init(argc, argv, "TRON dapter");
+ ros::init(argc, argv, "TRON adapter");
ros::NodeHandle nh;
try {
socket_fd = create_connected_socket(IP, PORT);
configuration_phase(nh);
-
request_start();
-
+
// testing phase loop
ros::Rate test_phase_freq(10);
while (ros::ok()) {