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 > 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 > 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