From cb6f4fe615d773f40c4a57dc80f9706330fdc91f Mon Sep 17 00:00:00 2001 From: CS <christoph.schroeter1@mailbox.tu-dresden.de> Date: Sun, 8 Aug 2021 20:11:56 +0200 Subject: [PATCH] --- cobot1_v1.xml | 69 ++++++++++++++++++++++++-------------------- src/tron_adapter.cpp | 2 +- 2 files changed, 39 insertions(+), 32 deletions(-) diff --git a/cobot1_v1.xml b/cobot1_v1.xml index 2ecf333..351ff74 100644 --- a/cobot1_v1.xml +++ b/cobot1_v1.xml @@ -16,7 +16,7 @@ chan move_to_start_pos;</declaration> <template> <name x="5" y="5">Cobot</name> <declaration>// Place local declarations here.</declaration> - <location id="id0" x="-510" y="-365"> + <location id="id0" x="-510" y="-280"> </location> <location id="id1" x="-340" y="-187"> </location> @@ -69,7 +69,14 @@ chan move_to_start_pos;</declaration> <location id="id21" x="272" y="212"> <committed/> </location> - <init ref="id0"/> + <location id="id22" x="-510" y="-408"> + </location> + <init ref="id22"/> + <transition> + <source ref="id22"/> + <target ref="id0"/> + <label kind="synchronisation" x="-654" y="-374">move_to_start_pos?</label> + </transition> <transition> <source ref="id7"/> <target ref="id15"/> @@ -261,98 +268,98 @@ chan move_to_start_pos;</declaration> <transition> <source ref="id0"/> <target ref="id9"/> - <label kind="select" x="-629" y="-340">i : int[5,10]</label> - <label kind="synchronisation" x="-637" y="-323">pressure_signal?</label> - <label kind="assignment" x="-731" y="-306">init_retry = i, retry = init_retry</label> + <label kind="select" x="-629" y="-289">i : int[5,10]</label> + <label kind="synchronisation" x="-646" y="-272">pressure_signal?</label> + <label kind="assignment" x="-731" y="-255">init_retry = i, retry = init_retry</label> </transition> </template> <template> <name>channels</name> - <location id="id22" x="-561" y="-144"> + <location id="id23" x="-561" y="-144"> <committed/> </location> - <init ref="id22"/> + <init ref="id23"/> <transition> - <source ref="id22"/> - <target ref="id22"/> + <source ref="id23"/> + <target ref="id23"/> <label kind="synchronisation" x="-697" y="-408">move_to_start_pos!</label> <nail x="-680" y="-391"/> <nail x="-595" y="-391"/> </transition> <transition> - <source ref="id22"/> - <target ref="id22"/> + <source ref="id23"/> + <target ref="id23"/> <label kind="synchronisation" x="-892" y="-382">glass_place_target!</label> <nail x="-561" y="-153"/> <nail x="-807" y="-340"/> <nail x="-748" y="-365"/> </transition> <transition> - <source ref="id22"/> - <target ref="id22"/> + <source ref="id23"/> + <target ref="id23"/> <label kind="synchronisation" x="-977" y="-297">glass_place_start!</label> <nail x="-867" y="-255"/> <nail x="-841" y="-297"/> </transition> <transition> - <source ref="id22"/> - <target ref="id22"/> + <source ref="id23"/> + <target ref="id23"/> <label kind="synchronisation" x="-960" y="-204">glass_pickup!</label> <nail x="-867" y="-187"/> <nail x="-858" y="-221"/> </transition> <transition> - <source ref="id22"/> - <target ref="id22"/> + <source ref="id23"/> + <target ref="id23"/> <label kind="synchronisation" x="-943" y="-102">bottle_place!</label> <nail x="-850" y="-93"/> <nail x="-850" y="-119"/> </transition> <transition> - <source ref="id22"/> - <target ref="id22"/> + <source ref="id23"/> + <target ref="id23"/> <label kind="synchronisation" x="-884" y="0">bottle_pickup!</label> <nail x="-782" y="-8"/> <nail x="-807" y="-34"/> </transition> <transition> - <source ref="id22"/> - <target ref="id22"/> + <source ref="id23"/> + <target ref="id23"/> <label kind="synchronisation" x="-739" y="85">stop_pour!</label> <nail x="-671" y="59"/> <nail x="-722" y="25"/> </transition> <transition> - <source ref="id22"/> - <target ref="id22"/> + <source ref="id23"/> + <target ref="id23"/> <label kind="synchronisation" x="-578" y="76">start_pour!</label> <nail x="-535" y="59"/> <nail x="-595" y="68"/> </transition> <transition> - <source ref="id22"/> - <target ref="id22"/> + <source ref="id23"/> + <target ref="id23"/> <label kind="synchronisation" x="-425" y="-17">move_to_glass_end!</label> <nail x="-433" y="0"/> <nail x="-493" y="42"/> </transition> <transition> - <source ref="id22"/> - <target ref="id22"/> + <source ref="id23"/> + <target ref="id23"/> <label kind="synchronisation" x="-340" y="-93">move_to_glass_start!</label> <nail x="-340" y="-110"/> <nail x="-340" y="-51"/> </transition> <transition> - <source ref="id22"/> - <target ref="id22"/> + <source ref="id23"/> + <target ref="id23"/> <label kind="synchronisation" x="-340" y="-221">move_to_bottle_pos!</label> <nail x="-340" y="-246"/> <nail x="-331" y="-187"/> </transition> <transition> - <source ref="id22"/> - <target ref="id22"/> + <source ref="id23"/> + <target ref="id23"/> <label kind="synchronisation" x="-425" y="-374">pressure_signal!</label> <nail x="-459" y="-382"/> <nail x="-391" y="-306"/> diff --git a/src/tron_adapter.cpp b/src/tron_adapter.cpp index dd67bae..fdedc99 100644 --- a/src/tron_adapter.cpp +++ b/src/tron_adapter.cpp @@ -445,7 +445,7 @@ bool compare_poses(geometry_msgs::Pose p1, geometry_msgs::Pose p2, double pos_to 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) + if (angle_diff < angle_tol && pos_diff < pos_tolerance) return true; return false; } -- GitLab