Skip to content
Snippets Groups Projects
Commit cb6f4fe6 authored by Christoph Schröter's avatar Christoph Schröter
Browse files

No commit message 

No commit message
parent eafb74d7
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
cobot1_v1.xml
+ 38
− 31
View file @ cb6f4fe6
...@@ -16,7 +16,7 @@ chan move_to_start_pos;</declaration> ...@@ -16,7 +16,7 @@ chan move_to_start_pos;</declaration>
<template> <template>
<name x="5" y="5">Cobot</name> <name x="5" y="5">Cobot</name>
<declaration>// Place local declarations here.</declaration> <declaration>// Place local declarations here.</declaration>
<location id="id0" x="-510" y="-365"> <location id="id0" x="-510" y="-280">
</location> </location>
<location id="id1" x="-340" y="-187"> <location id="id1" x="-340" y="-187">
</location> </location>
...@@ -69,7 +69,14 @@ chan move_to_start_pos;</declaration> ...@@ -69,7 +69,14 @@ chan move_to_start_pos;</declaration>
<location id="id21" x="272" y="212"> <location id="id21" x="272" y="212">
<committed/> <committed/>
</location> </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> <transition>
<source ref="id7"/> <source ref="id7"/>
<target ref="id15"/> <target ref="id15"/>
...@@ -261,98 +268,98 @@ chan move_to_start_pos;</declaration> ...@@ -261,98 +268,98 @@ chan move_to_start_pos;</declaration>
<transition> <transition>
<source ref="id0"/> <source ref="id0"/>
<target ref="id9"/> <target ref="id9"/>
<label kind="select" x="-629" y="-340">i : int[5,10]</label> <label kind="select" x="-629" y="-289">i : int[5,10]</label>
<label kind="synchronisation" x="-637" y="-323">pressure_signal?</label> <label kind="synchronisation" x="-646" y="-272">pressure_signal?</label>
<label kind="assignment" x="-731" y="-306">init_retry = i, retry = init_retry</label> <label kind="assignment" x="-731" y="-255">init_retry = i, retry = init_retry</label>
</transition> </transition>
</template> </template>
<template> <template>
<name>channels</name> <name>channels</name>
<location id="id22" x="-561" y="-144"> <location id="id23" x="-561" y="-144">
<committed/> <committed/>
</location> </location>
<init ref="id22"/> <init ref="id23"/>
<transition> <transition>
<source ref="id22"/> <source ref="id23"/>
<target ref="id22"/> <target ref="id23"/>
<label kind="synchronisation" x="-697" y="-408">move_to_start_pos!</label> <label kind="synchronisation" x="-697" y="-408">move_to_start_pos!</label>
<nail x="-680" y="-391"/> <nail x="-680" y="-391"/>
<nail x="-595" y="-391"/> <nail x="-595" y="-391"/>
</transition> </transition>
<transition> <transition>
<source ref="id22"/> <source ref="id23"/>
<target ref="id22"/> <target ref="id23"/>
<label kind="synchronisation" x="-892" y="-382">glass_place_target!</label> <label kind="synchronisation" x="-892" y="-382">glass_place_target!</label>
<nail x="-561" y="-153"/> <nail x="-561" y="-153"/>
<nail x="-807" y="-340"/> <nail x="-807" y="-340"/>
<nail x="-748" y="-365"/> <nail x="-748" y="-365"/>
</transition> </transition>
<transition> <transition>
<source ref="id22"/> <source ref="id23"/>
<target ref="id22"/> <target ref="id23"/>
<label kind="synchronisation" x="-977" y="-297">glass_place_start!</label> <label kind="synchronisation" x="-977" y="-297">glass_place_start!</label>
<nail x="-867" y="-255"/> <nail x="-867" y="-255"/>
<nail x="-841" y="-297"/> <nail x="-841" y="-297"/>
</transition> </transition>
<transition> <transition>
<source ref="id22"/> <source ref="id23"/>
<target ref="id22"/> <target ref="id23"/>
<label kind="synchronisation" x="-960" y="-204">glass_pickup!</label> <label kind="synchronisation" x="-960" y="-204">glass_pickup!</label>
<nail x="-867" y="-187"/> <nail x="-867" y="-187"/>
<nail x="-858" y="-221"/> <nail x="-858" y="-221"/>
</transition> </transition>
<transition> <transition>
<source ref="id22"/> <source ref="id23"/>
<target ref="id22"/> <target ref="id23"/>
<label kind="synchronisation" x="-943" y="-102">bottle_place!</label> <label kind="synchronisation" x="-943" y="-102">bottle_place!</label>
<nail x="-850" y="-93"/> <nail x="-850" y="-93"/>
<nail x="-850" y="-119"/> <nail x="-850" y="-119"/>
</transition> </transition>
<transition> <transition>
<source ref="id22"/> <source ref="id23"/>
<target ref="id22"/> <target ref="id23"/>
<label kind="synchronisation" x="-884" y="0">bottle_pickup!</label> <label kind="synchronisation" x="-884" y="0">bottle_pickup!</label>
<nail x="-782" y="-8"/> <nail x="-782" y="-8"/>
<nail x="-807" y="-34"/> <nail x="-807" y="-34"/>
</transition> </transition>
<transition> <transition>
<source ref="id22"/> <source ref="id23"/>
<target ref="id22"/> <target ref="id23"/>
<label kind="synchronisation" x="-739" y="85">stop_pour!</label> <label kind="synchronisation" x="-739" y="85">stop_pour!</label>
<nail x="-671" y="59"/> <nail x="-671" y="59"/>
<nail x="-722" y="25"/> <nail x="-722" y="25"/>
</transition> </transition>
<transition> <transition>
<source ref="id22"/> <source ref="id23"/>
<target ref="id22"/> <target ref="id23"/>
<label kind="synchronisation" x="-578" y="76">start_pour!</label> <label kind="synchronisation" x="-578" y="76">start_pour!</label>
<nail x="-535" y="59"/> <nail x="-535" y="59"/>
<nail x="-595" y="68"/> <nail x="-595" y="68"/>
</transition> </transition>
<transition> <transition>
<source ref="id22"/> <source ref="id23"/>
<target ref="id22"/> <target ref="id23"/>
<label kind="synchronisation" x="-425" y="-17">move_to_glass_end!</label> <label kind="synchronisation" x="-425" y="-17">move_to_glass_end!</label>
<nail x="-433" y="0"/> <nail x="-433" y="0"/>
<nail x="-493" y="42"/> <nail x="-493" y="42"/>
</transition> </transition>
<transition> <transition>
<source ref="id22"/> <source ref="id23"/>
<target ref="id22"/> <target ref="id23"/>
<label kind="synchronisation" x="-340" y="-93">move_to_glass_start!</label> <label kind="synchronisation" x="-340" y="-93">move_to_glass_start!</label>
<nail x="-340" y="-110"/> <nail x="-340" y="-110"/>
<nail x="-340" y="-51"/> <nail x="-340" y="-51"/>
</transition> </transition>
<transition> <transition>
<source ref="id22"/> <source ref="id23"/>
<target ref="id22"/> <target ref="id23"/>
<label kind="synchronisation" x="-340" y="-221">move_to_bottle_pos!</label> <label kind="synchronisation" x="-340" y="-221">move_to_bottle_pos!</label>
<nail x="-340" y="-246"/> <nail x="-340" y="-246"/>
<nail x="-331" y="-187"/> <nail x="-331" y="-187"/>
</transition> </transition>
<transition> <transition>
<source ref="id22"/> <source ref="id23"/>
<target ref="id22"/> <target ref="id23"/>
<label kind="synchronisation" x="-425" y="-374">pressure_signal!</label> <label kind="synchronisation" x="-425" y="-374">pressure_signal!</label>
<nail x="-459" y="-382"/> <nail x="-459" y="-382"/>
<nail x="-391" y="-306"/> <nail x="-391" y="-306"/>
......
src/tron_adapter.cpp
+ 1
− 1
View file @ cb6f4fe6
...@@ -445,7 +445,7 @@ bool compare_poses(geometry_msgs::Pose p1, geometry_msgs::Pose p2, double pos_to ...@@ -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 y_d = p1.position.y - p2.position.y;
double z_d = p1.position.z - p2.position.z; 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); 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 true;
return false; return false;
} }
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment