diff --git a/garage_actionlib.xml b/garage_actionlib.xml index 7ee98a8c3a3b054d1753e9b900c6b102d98ce953..fc3833c00bab022e770a4bee91123803a962f805 100644 --- a/garage_actionlib.xml +++ b/garage_actionlib.xml @@ -25,11 +25,31 @@ int lokal;</declaration> <location id="id3" x="204" y="85"> <name x="102" y="85">schliessend</name> </location> + <location id="id4" x="263" y="-51"> + </location> + <location id="id5" x="136" y="-85"> + </location> <init ref="id0"/> + <transition> + <source ref="id5"/> + <target ref="id3"/> + <label kind="select" x="85" y="-42">i : int[0,1000]</label> + <label kind="guard" x="42" y="-25">akt_position + i <= 1000</label> + <label kind="synchronisation" x="110" y="-59">fertig!</label> + <label kind="assignment" x="85" y="-8">akt_position += i</label> + </transition> + <transition> + <source ref="id4"/> + <target ref="id1"/> + <label kind="select" x="221" y="-127">i : int[0,1000]</label> + <label kind="guard" x="204" y="-110">akt_position - i >= 0</label> + <label kind="synchronisation" x="238" y="-144">fertig!</label> + <label kind="assignment" x="229" y="-93">akt_position -= i</label> + </transition> <transition> <source ref="id0"/> <target ref="id3"/> - <label kind="synchronisation" x="-391" y="0">schliessen?</label> + <label kind="synchronisation" x="-365" y="0">schliessen?</label> <nail x="-280" y="-76"/> <nail x="-280" y="93"/> </transition> @@ -42,15 +62,13 @@ int lokal;</declaration> </transition> <transition> <source ref="id3"/> - <target ref="id1"/> - <label kind="synchronisation" x="280" y="-85">oeffnen?</label> - <nail x="280" y="-68"/> + <target ref="id4"/> + <label kind="synchronisation" x="246" y="-17">oeffnen?</label> </transition> <transition> <source ref="id1"/> - <target ref="id3"/> - <label kind="synchronisation" x="34" y="-110">schliessen?</label> - <nail x="119" y="-68"/> + <target ref="id5"/> + <label kind="synchronisation" x="76" y="-153">schliessen?</label> </transition> <transition> <source ref="id3"/> @@ -58,7 +76,7 @@ int lokal;</declaration> <label kind="select" x="59" y="170">i : int[0,1000]</label> <label kind="guard" x="59" y="187">akt_position - i >= 0</label> <label kind="synchronisation" x="59" y="153">position!</label> - <label kind="assignment" x="59" y="204">akt_position = akt_position - i</label> + <label kind="assignment" x="59" y="204">akt_position -= i</label> <nail x="280" y="178"/> <nail x="144" y="178"/> </transition> @@ -68,7 +86,7 @@ int lokal;</declaration> <label kind="select" x="305" y="-356">i : int[0,1000]</label> <label kind="guard" x="305" y="-339">akt_position + i <= 1000</label> <label kind="synchronisation" x="305" y="-373">position!</label> - <label kind="assignment" x="305" y="-322">akt_position = akt_position +i</label> + <label kind="assignment" x="305" y="-322">akt_position += i</label> <nail x="126" y="-335"/> <nail x="126" y="-335"/> <nail x="270" y="-335"/> @@ -76,7 +94,10 @@ int lokal;</declaration> <transition> <source ref="id3"/> <target ref="id0"/> - <label kind="synchronisation" x="-143" y="0">fertig!</label> + <label kind="select" x="-127" y="-8">i : int[0,1000]</label> + <label kind="guard" x="-127" y="8">akt_position - i >= 0</label> + <label kind="synchronisation" x="-93" y="-25">fertig!</label> + <label kind="assignment" x="-127" y="25">akt_position -= i</label> </transition> <transition> <source ref="id2"/> @@ -86,7 +107,10 @@ int lokal;</declaration> <transition> <source ref="id1"/> <target ref="id2"/> - <label kind="synchronisation" x="289" y="-195">fertig!</label> + <label kind="select" x="348" y="-187">i : int[0,1000]</label> + <label kind="guard" x="348" y="-170">akt_position + i <= 1000</label> + <label kind="synchronisation" x="348" y="-204">fertig!</label> + <label kind="assignment" x="357" y="-153">akt_position += i</label> </transition> <transition> <source ref="id0"/> @@ -97,12 +121,12 @@ int lokal;</declaration> <template> <name>Schluessel</name> <declaration>clock zeit;</declaration> - <location id="id4" x="-1045" y="-722"> + <location id="id6" x="-1045" y="-722"> </location> - <init ref="id4"/> + <init ref="id6"/> <transition> - <source ref="id4"/> - <target ref="id4"/> + <source ref="id6"/> + <target ref="id6"/> <label kind="guard" x="-1215" y="-765">zeit > 10</label> <label kind="synchronisation" x="-1207" y="-739">schliessen!</label> <label kind="assignment" x="-1224" y="-714">zeit = 0</label> @@ -111,8 +135,8 @@ int lokal;</declaration> <nail x="-1045" y="-578"/> </transition> <transition> - <source ref="id4"/> - <target ref="id4"/> + <source ref="id6"/> + <target ref="id6"/> <label kind="guard" x="-935" y="-850">zeit > 10</label> <label kind="synchronisation" x="-977" y="-867">oeffnen!</label> <label kind="assignment" x="-909" y="-824">zeit = 0</label> diff --git a/src/garage_server.cpp b/src/garage_server.cpp index 53f92fe99cdaea47aa560270591f183e4a12d41e..3add00e3ce196f4a38ed229de20ac8dcb1c5368e 100644 --- a/src/garage_server.cpp +++ b/src/garage_server.cpp @@ -39,6 +39,7 @@ class Garage { rate.sleep(); if (as.isPreemptRequested() || !ros::ok() || as.isNewGoalAvailable()) { ROS_INFO("server preempted"); + res.position = current_position; as.setPreempted(res, "preempted"); return; } diff --git a/src/tron_adapter.cpp b/src/tron_adapter.cpp index c6c4c1e2038b50aeb22214bf00a63b601fefee3e..b8e54aae4078af2d8bc634805eb0cda6d2bb53ca 100644 --- a/src/tron_adapter.cpp +++ b/src/tron_adapter.cpp @@ -397,8 +397,7 @@ void feedback_callback(const boost::shared_ptr<actionlib_example::TriggerActionF } void result_callback(const boost::shared_ptr<actionlib_example::TriggerActionResult> ptr){ for (Mapping& map : mappings) { - if (map.channel.name == "fertig" && map.channel.is_input == false - && ptr->status.status == 3) // 3 means succeeded, could alternively be implemented in uppaal model + if (map.channel.name == "fertig" && map.channel.is_input == false) report_now(map.channel, 1, &ptr->result.position); } } @@ -434,7 +433,7 @@ void configuration_phase(ros::NodeHandle& nh){ // add_var_to_channel(socketfd, "ausloesen", "lokal"); uint64_t microseconds = 1000000; // one second // documentation states 2 signed integers are used for some reason - set_time_unit_and_timeout(microseconds, 100); + set_time_unit_and_timeout(microseconds, 300); // wait till subscribers initialized for (ros::Publisher& pub : input_publishers) { @@ -522,7 +521,7 @@ int main(int argc, char**argv){ ros::AsyncSpinner spinner(4); spinner.start(); while (ros::ok()) { - //ros::spinOnce(); + ros::spinOnce(); process_TRONs_msgs(); test_phase_freq.sleep(); }