I nodi non sono segnati come Required

Spegnimento grafecul attraverso interfaccia
Modifica angoli RPY per tenere utensile perpendicolare al terreno
2019-11-19 09:21:38 +01:00 · 2019-11-19 09:20:45 +01:00 · 2019-11-15 17:01:45 +01:00 · 2019-11-15 16:45:29 +01:00 · 2019-11-15 14:46:16 +01:00 · 2019-11-15 12:22:50 +01:00
17 changed files with 786 additions and 197 deletions
--- a/roboglue_ros_ws.workspace.user
+++ b/roboglue_ros_ws.workspace.user
@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 4.9.1, 2019-10-23T14:10:27. -->
+<!-- Written by QtCreator 4.9.2, 2019-11-15T17:00:10. -->
 <qtcreator>
 <data>
  <variable>EnvironmentId</variable>
@@ -72,7 +72,7 @@
   <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{b917fd33-cfca-4cf9-a8d9-025510846768}</value>
   <value type="int" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
   <value type="int" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
-   <value type="int" key="ProjectExplorer.Target.ActiveRunConfiguration">2</value>
+   <value type="int" key="ProjectExplorer.Target.ActiveRunConfiguration">1</value>
   <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
    <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory"></value>
    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
@@ -124,7 +124,7 @@
     <value type="QString">ROS_PACKAGE_PATH=/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src:/opt/ros/kinetic/share</value>
     <value type="QString">ROS_ROOT=/opt/ros/kinetic/share/ros</value>
     <value type="QString">ROS_VERSION=1</value>
-     <value type="QString">SHLVL=194</value>
+     <value type="QString">SHLVL=940</value>
     <value type="QString">TERM=xterm</value>
     <value type="QString">_=/usr/bin/env</value>
    </valuelist>
@@ -483,7 +483,20 @@
      <value type="QString" key="ROSProjectManager.ROSGenericStep.Target">roboglue_ros_urdriver.launch</value>
      <value type="QString" key="ROSProjectManager.ROSGenericStep.TargetPath">/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros/launch/roboglue_ros_urdriver.launch</value>
     </valuemap>
-     <value type="int" key="ProjectExplorer.RunStepList.StepsCount">1</value>
+     <valuemap type="QVariantMap" key="ProjectExplorer.RunStepList.Step.1">
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ROSProjectManager.ROSAttachToNode</value>
+      <value type="bool" key="ProjectExplorer.RunStep.Enabled">true</value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.Arguments"></value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.Command">debug</value>
+      <value type="bool" key="ROSProjectManager.ROSGenericStep.DebugContinueOnAttach">false</value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.Package">roboglue_ros</value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.PackagePath">/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros</value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.Target">roboglue_ros_recorder</value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.TargetPath">/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/devel/lib/roboglue_ros/roboglue_ros_recorder</value>
+     </valuemap>
+     <value type="int" key="ProjectExplorer.RunStepList.StepsCount">2</value>
    </valuemap>
    <value type="uint" key="RunConfiguration.QmlDebugServerPort">3768</value>
    <value type="bool" key="RunConfiguration.UseCppDebugger">false</value>
@@ -550,6 +563,19 @@
     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ROSProjectManager.RunStepList</value>
     <valuemap type="QVariantMap" key="ProjectExplorer.RunStepList.Step.0">
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ROSProjectManager.ROSAttachToNode</value>
+      <value type="bool" key="ProjectExplorer.RunStep.Enabled">true</value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.Arguments"></value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.Command">debug</value>
+      <value type="bool" key="ROSProjectManager.ROSGenericStep.DebugContinueOnAttach">true</value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.Package">roboglue_ros</value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.PackagePath">/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros</value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.Target">roboglue_ros_relay</value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.TargetPath">/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/devel/lib/roboglue_ros/roboglue_ros_relay</value>
+     </valuemap>
+     <valuemap type="QVariantMap" key="ProjectExplorer.RunStepList.Step.1">
      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName"></value>
      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ROSProjectManager.ROSLaunchStep</value>
@@ -562,7 +588,7 @@
      <value type="QString" key="ROSProjectManager.ROSGenericStep.Target">roboglue_ros_urdriver_norviz.launch</value>
      <value type="QString" key="ROSProjectManager.ROSGenericStep.TargetPath">/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch</value>
     </valuemap>
-     <value type="int" key="ProjectExplorer.RunStepList.StepsCount">1</value>
+     <value type="int" key="ProjectExplorer.RunStepList.StepsCount">2</value>
    </valuemap>
    <value type="uint" key="RunConfiguration.QmlDebugServerPort">3768</value>
    <value type="bool" key="RunConfiguration.UseCppDebugger">true</value>
@@ -571,7 +597,99 @@
    <value type="bool" key="RunConfiguration.UseQmlDebugger">false</value>
    <value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">false</value>
   </valuemap>
-   <value type="int" key="ProjectExplorer.Target.RunConfigurationCount">3</value>
+   <valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.3">
+    <value type="QString" key="Analyzer.Perf.CallgraphMode">dwarf</value>
+    <valuelist type="QVariantList" key="Analyzer.Perf.Events">
+     <value type="QString">cpu-cycles</value>
+    </valuelist>
+    <valuelist type="QVariantList" key="Analyzer.Perf.ExtraArguments"/>
+    <value type="int" key="Analyzer.Perf.Frequency">250</value>
+    <value type="QString" key="Analyzer.Perf.SampleMode">-F</value>
+    <value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
+    <value type="int" key="Analyzer.Perf.StackSize">4096</value>
+    <value type="bool" key="Analyzer.QmlProfiler.AggregateTraces">false</value>
+    <value type="bool" key="Analyzer.QmlProfiler.FlushEnabled">false</value>
+    <value type="uint" key="Analyzer.QmlProfiler.FlushInterval">1000</value>
+    <value type="QString" key="Analyzer.QmlProfiler.LastTraceFile"></value>
+    <value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
+    <valuelist type="QVariantList" key="Analyzer.Valgrind.AddedSuppressionFiles"/>
+    <value type="bool" key="Analyzer.Valgrind.Callgrind.CollectBusEvents">false</value>
+    <value type="bool" key="Analyzer.Valgrind.Callgrind.CollectSystime">false</value>
+    <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableBranchSim">false</value>
+    <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableCacheSim">false</value>
+    <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableEventToolTips">true</value>
+    <value type="double" key="Analyzer.Valgrind.Callgrind.MinimumCostRatio">0.01</value>
+    <value type="double" key="Analyzer.Valgrind.Callgrind.VisualisationMinimumCostRatio">10</value>
+    <value type="bool" key="Analyzer.Valgrind.FilterExternalIssues">true</value>
+    <value type="QString" key="Analyzer.Valgrind.KCachegrindExecutable">kcachegrind</value>
+    <value type="int" key="Analyzer.Valgrind.LeakCheckOnFinish">1</value>
+    <value type="int" key="Analyzer.Valgrind.NumCallers">25</value>
+    <valuelist type="QVariantList" key="Analyzer.Valgrind.RemovedSuppressionFiles"/>
+    <value type="int" key="Analyzer.Valgrind.SelfModifyingCodeDetection">1</value>
+    <value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
+    <value type="bool" key="Analyzer.Valgrind.ShowReachable">false</value>
+    <value type="bool" key="Analyzer.Valgrind.TrackOrigins">true</value>
+    <value type="QString" key="Analyzer.Valgrind.ValgrindExecutable">valgrind</value>
+    <valuelist type="QVariantList" key="Analyzer.Valgrind.VisibleErrorKinds">
+     <value type="int">0</value>
+     <value type="int">1</value>
+     <value type="int">2</value>
+     <value type="int">3</value>
+     <value type="int">4</value>
+     <value type="int">5</value>
+     <value type="int">6</value>
+     <value type="int">7</value>
+     <value type="int">8</value>
+     <value type="int">9</value>
+     <value type="int">10</value>
+     <value type="int">11</value>
+     <value type="int">12</value>
+     <value type="int">13</value>
+     <value type="int">14</value>
+    </valuelist>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">ROS Run Configuration</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">RoboGlueRUN_test</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ROSProjectManager.ROSRunConfigurationThis is a test</value>
+    <valuemap type="QVariantMap" key="ROSProjectManager.RunStepList">
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Run</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ROSProjectManager.RunStepList</value>
+     <valuemap type="QVariantMap" key="ProjectExplorer.RunStepList.Step.0">
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ROSProjectManager.ROSAttachToNode</value>
+      <value type="bool" key="ProjectExplorer.RunStep.Enabled">true</value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.Arguments"></value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.Command">debug</value>
+      <value type="bool" key="ROSProjectManager.ROSGenericStep.DebugContinueOnAttach">false</value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.Package">roboglue_ros</value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.PackagePath">/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros</value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.Target">roboglue_ros_test</value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.TargetPath">/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/devel/lib/roboglue_ros/roboglue_ros_test</value>
+     </valuemap>
+     <valuemap type="QVariantMap" key="ProjectExplorer.RunStepList.Step.1">
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ROSProjectManager.ROSLaunchStep</value>
+      <value type="bool" key="ProjectExplorer.RunStep.Enabled">true</value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.Arguments"></value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.Command">roslaunch</value>
+      <value type="bool" key="ROSProjectManager.ROSGenericStep.DebugContinueOnAttach">true</value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.Package">roboglue_ros</value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.PackagePath">/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros</value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.Target">test.launch</value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.TargetPath">/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros/launch/test.launch</value>
+     </valuemap>
+     <value type="int" key="ProjectExplorer.RunStepList.StepsCount">2</value>
+    </valuemap>
+    <value type="uint" key="RunConfiguration.QmlDebugServerPort">3768</value>
+    <value type="bool" key="RunConfiguration.UseCppDebugger">false</value>
+    <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
+    <value type="bool" key="RunConfiguration.UseMultiProcess">false</value>
+    <value type="bool" key="RunConfiguration.UseQmlDebugger">false</value>
+    <value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
+   </valuemap>
+   <value type="int" key="ProjectExplorer.Target.RunConfigurationCount">4</value>
  </valuemap>
 </data>
 <data>
--- a/src/roboglue_ros/CMakeLists.txt
+++ b/src/roboglue_ros/CMakeLists.txt
@@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
  moveit_ros_planning
  moveit_ros_planning_interface
  moveit_visual_tools
+  tf
  )

 find_package (Eigen3 REQUIRED)
@@ -148,6 +149,11 @@ include_directories(
     src/roboglue_follower.cpp
     src/roboglue_utils.cpp
     )
+ add_executable(
+     ${PROJECT_NAME}_broadcaster
+     src/roboglue_broadcaster.cpp
+     src/roboglue_utils.cpp
+     )
 add_executable (
     ${PROJECT_NAME}_test
     src/roboglue_test.cpp
@@ -177,6 +183,9 @@ target_link_libraries(
 target_link_libraries(
    ${PROJECT_NAME}_follower ${catkin_LIBRARIES}
 )
+target_link_libraries(
+    ${PROJECT_NAME}_broadcaster ${catkin_LIBRARIES}
+)


 #############
--- a/src/roboglue_ros/config/data/test99.data
+++ b/src/roboglue_ros/config/data/test99.data
@@ -0,0 +1,51 @@
+N,X,Y,Z,rR,rP,rY,DT,DS
+N0,0.018000,0.306000,0.500000,0.000000,1.571000,1.571000,0.000000,0.000000
+N1,0.018000,0.312000,0.500000,0.000000,1.571000,1.571000,0.095493,0.006000
+N2,0.018000,0.318000,0.500000,0.000000,1.571000,1.571000,0.177313,0.006000
+N3,0.018000,0.324000,0.500000,0.000000,1.571000,1.571000,0.276251,0.006000
+N4,-0.006000,0.324000,0.500000,0.000000,1.571000,1.571000,0.376993,0.024000
+N5,-0.024000,0.312000,0.500000,0.000000,1.571000,1.571000,0.477276,0.021633
+N6,-0.030000,0.288000,0.500000,0.000000,1.571000,1.571000,0.583836,0.024739
+N7,-0.048000,0.258000,0.500000,0.000000,1.571000,1.571000,0.674398,0.034986
+N8,-0.060000,0.246000,0.500000,0.000000,1.571000,1.571000,0.779036,0.016971
+N9,-0.066000,0.240000,0.500000,0.000000,1.571000,1.571000,0.878682,0.008485
+N10,-0.078000,0.240000,0.500000,0.000000,1.571000,1.571000,0.985584,0.012000
+N11,-0.126000,0.240000,0.500000,0.000000,1.571000,1.571000,1.076938,0.048000
+N12,-0.150000,0.240000,0.500000,0.000000,1.571000,1.571000,1.173122,0.024000
+N13,-0.162000,0.252000,0.500000,0.000000,1.571000,1.571000,1.278250,0.016971
+N14,-0.174000,0.258000,0.500000,0.000000,1.571000,1.571000,1.378685,0.013416
+N15,-0.186000,0.276000,0.500000,0.000000,1.571000,1.571000,1.484213,0.021633
+N16,-0.198000,0.300000,0.500000,0.000000,1.571000,1.571000,0.006499,0.026833
+N17,-0.204000,0.324000,0.500000,0.000000,1.571000,1.571000,1.690293,0.024739
+N18,-0.204000,0.342000,0.500000,0.000000,1.571000,1.571000,1.780065,0.018000
+N19,-0.204000,0.378000,0.500000,0.000000,1.571000,1.571000,1.872321,0.036000
+N20,-0.222000,0.450000,0.500000,0.000000,1.571000,1.571000,1.977848,0.074216
+N21,-0.318000,0.468000,0.500000,0.000000,1.571000,1.571000,2.070969,0.097673
+N22,-0.396000,0.450000,0.500000,0.000000,1.571000,1.571000,2.177687,0.080050
+N23,-0.462000,0.348000,0.500000,0.000000,1.571000,1.571000,2.283137,0.121491
+N24,-0.558000,0.264000,0.500000,0.000000,1.571000,1.571000,2.392030,0.127562
+N25,-0.636000,0.246000,0.500000,0.000000,1.571000,1.571000,2.479188,0.080050
+N26,-0.702000,0.306000,0.500000,0.000000,1.571000,1.571000,2.581360,0.089196
+N27,-0.702000,0.402000,0.500000,0.000000,1.571000,1.571000,2.672705,0.096000
+N28,-0.618000,0.480000,0.500000,0.000000,1.571000,1.571000,2.780175,0.114630
+N29,-0.540000,0.468000,0.500000,0.000000,1.571000,1.571000,2.887023,0.078918
+N30,-0.300000,0.384000,0.500000,0.000000,1.571000,1.571000,2.980252,0.254275
+N31,-0.126000,0.336000,0.500000,0.000000,1.571000,1.571000,3.088443,0.180499
+N32,-0.012000,0.318000,0.500000,0.000000,1.571000,1.571000,3.172616,0.115412
+N33,0.042000,0.372000,0.500000,0.000000,1.571000,1.571000,3.277146,0.076368
+N34,0.042000,0.396000,0.500000,0.000000,1.571000,1.571000,3.385895,0.024000
+N35,0.024000,0.390000,0.500000,0.000000,1.571000,1.571000,3.478245,0.018974
+N36,0.000000,0.288000,0.500000,0.000000,1.571000,1.571000,3.585851,0.104785
+N37,0.048000,0.234000,0.500000,0.000000,1.571000,1.571000,3.673290,0.072250
+N38,0.198000,0.252000,0.500000,0.000000,1.571000,1.571000,3.786958,0.151076
+N39,0.252000,0.306000,0.500000,0.000000,1.571000,1.571000,3.881606,0.076368
+N40,0.264000,0.336000,0.500000,0.000000,1.571000,1.571000,3.977590,0.032311
+N41,0.330000,0.234000,0.500000,0.000000,1.571000,1.571000,4.082062,0.121491
+N42,0.330000,0.162000,0.500000,0.000000,1.571000,1.571000,4.177567,0.072000
+N43,0.336000,0.102000,0.500000,0.000000,1.571000,1.571000,4.284022,0.060299
+N44,0.396000,0.096000,0.500000,0.000000,1.571000,1.571000,4.377034,0.060299
+N45,0.450000,0.144000,0.500000,0.000000,1.571000,1.571000,4.494896,0.072250
+N46,0.456000,0.252000,0.500000,0.000000,1.571000,1.571000,4.604838,0.108167
+N47,0.420000,0.300000,0.500000,0.000000,1.571000,1.571000,4.684846,0.060000
+N48,0.378000,0.324000,0.500000,0.000000,1.571000,1.571000,4.774341,0.048374
+N49,0.348000,0.324000,0.500000,0.000000,1.571000,1.571000,4.876254,0.030000
--- a/src/roboglue_ros/config/meta/test09.meta
+++ b/src/roboglue_ros/config/meta/test09.meta
@@ -1 +1 @@
-{"code":"{846e5f31-1384-437f-bfd3-e7c49b8adc9a}","ds":100.0,"dt":0.5,"frame_base":"---","frame_work":{"name":"---","offset":{"r":0.0,"x":0.0,"y":0.0,"z":0.0}},"n_analog":0,"n_digital":0,"n_point":270,"name":"test09","type":"time/dist"}
+{"code":"{846e5f31-1384-437f-bfd3-e7c49b8adc9a}","ds":2.0,"dt":0.5,"frame_base":"---","frame_work":{"name":"---","offset":{"r":0.0,"x":0.0,"y":0.0,"z":0.0}},"n_analog":0,"n_digital":0,"n_point":270,"name":"test09","type":"time/dist"}
--- a/src/roboglue_ros/config/meta/test99.meta
+++ b/src/roboglue_ros/config/meta/test99.meta
@@ -0,0 +1 @@
+{"code":"{613d5288-5d28-465d-996d-db6750a6789c}","ds":0.5,"dt":0.5,"frame_base":"---","frame_work":{"name":"---","offset":{"r":0.0,"x":0.0,"y":0.0,"z":0.0}},"n_analog":0,"n_digital":0,"n_point":50,"name":"test99","type":"time/dist"}
--- a/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h
+++ b/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h
@@ -3,6 +3,7 @@

 #endif // ROBOGLUE_UTILS_H

+#include <signal.h>
 #include <iostream>
 #include <stdlib.h>
 #include <ros/ros.h>
@@ -35,18 +36,27 @@ namespace mvt=moveit_visual_tools;

 ///////// MQTT & ROS TOPIC NAMES ///////////////
 typedef struct {
-    std::string MQTTcommandPub, MQTTcoordPub, MQTTstatePub;
-    std::string MQTTcommandSub, MQTTcoordSub, MQTTstateSub;
+    std::string MQTTcommandPub, MQTTcoordPub, MQTTstatePub, MQTTinterfacePub;
+    std::string MQTTcommandSub, MQTTcoordSub, MQTTstateSub, MQTTinterfaceSub;
    std::string mqttHost;
    int mqttQoS;
 } MQTT_topics;

 typedef struct {
-    std::string commandPub, coordPub, statePub;
-    std::string commandSub, coordSub, stateSub;
+    std::string commandPub, coordPub, statePub, interfacePub;
+    std::string commandSub, coordSub, stateSub, interfaceSub;
    std::string delta_jog, delta_joint_jog, traj_jog;
 } ROS_topics;

+///////// COMMON PARAMETERS //////////////
+typedef struct {
+    ROS_topics ros_topics;
+    int loopRate, msgBufferSize;
+    std::string param_ns;
+    std_msgs::String startupMsg, shutdownMsg;
+    std::string defaultFrameName;
+} COMMON_parameters;
+
 ///////// AUX DATA STORAGE ///////////////
 typedef struct {
    std::list<std::pair<std::string,bool>> digital;
@@ -68,12 +78,15 @@ typedef struct{
    bool isRecording = false;
    bool isRealtime = false;
    bool isRunning = true;
+    bool setFrame = false;
 } internalState;

 ////////// POSITION DATA STORAGE ///////////
 typedef struct {
    geometry_msgs::TwistStamped twist;
    trajectory_msgs::JointTrajectory joint;
+    geometry_msgs::Pose activeFrame;
+    std::string activeFrameName;
    bool waitIK = false;
    bool isFirst = false;
    bool isNew = false;
@@ -98,12 +111,35 @@ typedef struct {
    robot_state::RobotStatePtr kine_state;
    std::vector<std::string> joint_names;
    std::map<std::string, float> joint_homes;
+    double joint_jump_tresh;
 } robotData;

+///////// FILE DATA STORAGE /////////////
+typedef struct {
+    nlohmann::json* metaFile = nullptr;
+    rapidcsv::Document* dataFile = nullptr;
+    std::string meta_dir_name;
+    std::string data_dir_name;
+    std::string meta_template_name;
+    std::string data_template_name;
+    std::string meta_ext;
+    std::string data_ext;
+    std::vector<pointRecord>* recordVect;
+    std::vector<pointRecord>* playVect;
+} fileData;
+// posizione degli elementi nella riga del csv
+enum VPOS {X,Y,Z,RX,RY,RZ,DT,DS,VMAX};
+
 typedef struct {
    mvt::MoveItVisualToolsPtr vtools;
 } rvizData;

+///////// LOAD COMMON ROS TOPICS AND PARAMETERS /////////////////////
+bool loadCommonParameters(ros::NodeHandle*, COMMON_parameters*);
+
+void declareStartup(publisherMap*, COMMON_parameters*);
+void declareShutdown(publisherMap*, COMMON_parameters*);
+
 geometry_msgs::TwistStamped jstr2Twist(nlohmann::json);
 trajectory_msgs::JointTrajectory jstr2Joint(nlohmann::json);

@@ -114,4 +150,4 @@ std::map<std::string, double> floatMap2doubleMap(std::map<std::string,float>);
 double e3dist(geometry_msgs::Twist, geometry_msgs::Twist);
 double rad2deg(double ang);
 double deg2rad(double ang);
-bool checkJointJump(moveit_msgs::RobotTrajectoryPtr, double);
+bool checkJointJump(moveit_msgs::RobotTrajectory*, double);
--- a/src/roboglue_ros/launch/roboglue_ros.launch
+++ b/src/roboglue_ros/launch/roboglue_ros.launch
@@ -29,6 +29,7 @@
        <param name="move_group_name" type="string" value="manipulator" />
        <param name="loop_rate" type="int" value="100" />
        <param name="msg_buffer" type="int" value="100" />
+        <param name="joint_jump_tresh" type="double" value="45.0" />
        <param name="min_nonzero_move" type="double" value="0.005" />
    </group>

--- a/src/roboglue_ros/launch/roboglue_ros_urdriver.launch
+++ b/src/roboglue_ros/launch/roboglue_ros_urdriver.launch
@@ -1,6 +1,6 @@
 <launch>
    <!-- Set logger format -->
-    <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${node}]: ${message}" />
+    <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${node}][${line}]: ${message}" />
    <!--    <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${node}] [${function}]: ${message}" />-->

    <!-- Start ROS official URobotics controller -->
@@ -13,27 +13,34 @@
        <!-- <arg name="robot_ip" value="10.0.0.5" /> -->
    </include>

-    <!-- Start rViz API interface-->
-    <include file="$(find ur10_e_moveit_config)/launch/moveit_rviz.launch">
-        <arg name="config" value="true" />
-    </include>
-
    <!-- Common parameter Server -->
    <group ns="roboglue_ros_common">
+        <!-- ROS internal topics -->
        <param name="INstate"   type="string" value="/roboglue_com/com2ros/state" />
        <param name="INcommands" type="string" value="/roboglue_com/com2ros/commands" />
        <param name="INcoordinates" type="string" value="/roboglue_com/com2ros/coordinates" />
+        <param name="INinterface" type="string" value="/roboglue_com/com2ros/interface" />
        <param name="OUTstate" type="string" value="/roboglue_com/ros2com/state" />
        <param name="OUTcommands" type="string" value="/roboglue_com/ros2com/commands" />
        <param name="OUTcoordinates" type="string" value="/roboglue_com/ros2com/coordinates" />
+        <param name="OUTinterface" type="string" value="/roboglue_com/ros2com/interface" />
+        <!-- ROS movement topics (For UR official driver use scalable position controller) -->
        <param name="OUTdelta_jog" type="string" value="/jog_arm_server/delta_jog_cmds" />
        <param name="OUTdelta_joint_jog" type="string" value="/jog_arm_server/delta_joint_jog_cmds" />
-        <param name="OUTtraj_jog" type="string" value="/pos_based_pos_traj_controller/command" />
+        <param name="OUTtraj_jog" type="string" value="/scaled_pos_traj_controller/command" />
+        <!-- ROS robot model description and defaults -->
        <param name="robot_model_name" type="string" value="robot_description" />
        <param name="move_group_name" type="string" value="manipulator" />
+        <param name="min_nonzero_move" type="double" value="0.005" />
+        <param name="joint_jump_tresh" type="double" value="45.0" />
+        <param name="default_frame_name" type="string" value= "/world"/>
+        <rosparam command="load" file="$(find roboglue_ros)/config/jointDefaults.yaml" />
+        <!-- ROS node loop parameters -->
+        <param name="startup_msg" type="string" value="STARTUP"/>
+        <param name="shutdown_msg" type="string" value="SHUTDOWN"/>
+        <param name="heartbeat_msg" type="string" value="HB"/>
        <param name="loop_rate" type="int" value="100" />
        <param name="msg_buffer" type="int" value="100" />
-        <param name="min_nonzero_move" type="double" value="0.005" />
    </group>

    <!-- Message Relay to/from MQTT -->
@@ -44,19 +51,27 @@
        <param name="mqtt_INstate" type="string" value="roboglue_com/com2ros/state" />
        <param name="mqtt_INcommands" type="string" value="roboglue_com/com2ros/commands" />
        <param name="mqtt_INcoordinates" type="string" value="roboglue_com/com2ros/coordinates" />
+        <param name="mqtt_INinterface" type="string" value="roboglue_com/com2ros/interface" />
        <param name="mqtt_OUTstate" type="string" value="roboglue_com/ros2com/state" />
        <param name="mqtt_OUTcommands" type="string" value="roboglue_com/ros2com/commands" />
        <param name="mqtt_OUTcoordinates" type="string" value="roboglue_com/ros2com/coordinates" />
+        <param name="mqtt_OUTinterface" type="string" value="roboglue_com/ros2com/interface" />
+    </node>
+
+    <!-- Frame Transform Broadcaster -->
+    <node name="roboglue_ros_broadcaster" pkg="roboglue_ros" type="roboglue_ros_broadcaster" output="screen" required="false">
+        <param name="parameter_ns" type="string" value="/roboglue_ros_common/" />
+        <param name="tf_pub_rate" type="int" value="10" />
    </node>

    <!-- Real Time Tracking -->
-    <node name="roboglue_ros_follower" pkg="roboglue_ros" type="roboglue_ros_follower" output="screen" required="true">
-        <param name="parameter_ns" type="string" value="roboglue_ros_common" />
+    <node name="roboglue_ros_follower" pkg="roboglue_ros" type="roboglue_ros_follower" output="screen" required="false">
+        <param name="parameter_ns" type="string" value="/roboglue_ros_common/" />
        <param name="jog_pub_rate" type="int" value="5" />
    </node>

    <!-- Tracking data Recorder/Player -->
-    <node name="roboglue_ros_recorder" pkg="roboglue_ros" type="roboglue_ros_recorder" output="screen" required="true">
+    <node name="roboglue_ros_recorder" pkg="roboglue_ros" type="roboglue_ros_recorder" output="screen" required="false">
        <param name="parameter_ns" type="string" value="/roboglue_ros_common/" />
        <param name="point_group_mode" type="string" value="dist" />
        <param name="planning_mode" type="string" value="path" />
@@ -68,7 +83,9 @@
        <param name="data_template" type="string" value="@data_template.data" />
    </node>

-    <!-- <param name="planning_mode" type="string" value="joint" />   -->
-    <!-- <param name="point_group_mode" type="string" value="time" /> -->
+    <!-- Start rViz API interface-->
+    <include file="$(find ur10_e_moveit_config)/launch/moveit_rviz.launch">
+        <arg name="config" value="true" />
+    </include>

 </launch>
--- a/src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch
+++ b/src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch
@@ -1,6 +1,6 @@
 <launch>
    <!-- Set logger format -->
-    <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${node}]: ${message}" />
+    <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${node}][${line}]: ${message}" />
    <!--    <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${node}] [${function}]: ${message}" />-->

    <!-- Start ROS official URobotics controller -->
@@ -19,9 +19,11 @@
        <param name="INstate"   type="string" value="/roboglue_com/com2ros/state" />
        <param name="INcommands" type="string" value="/roboglue_com/com2ros/commands" />
        <param name="INcoordinates" type="string" value="/roboglue_com/com2ros/coordinates" />
+        <param name="INinterface" type="string" value="/roboglue_com/com2ros/interface" />
        <param name="OUTstate" type="string" value="/roboglue_com/ros2com/state" />
        <param name="OUTcommands" type="string" value="/roboglue_com/ros2com/commands" />
        <param name="OUTcoordinates" type="string" value="/roboglue_com/ros2com/coordinates" />
+        <param name="OUTinterface" type="string" value="/roboglue_com/ros2com/interface" />
        <!-- ROS movement topics (For UR official driver use scalable position controller) -->
        <param name="OUTdelta_jog" type="string" value="/jog_arm_server/delta_jog_cmds" />
        <param name="OUTdelta_joint_jog" type="string" value="/jog_arm_server/delta_joint_jog_cmds" />
@@ -30,8 +32,13 @@
        <param name="robot_model_name" type="string" value="robot_description" />
        <param name="move_group_name" type="string" value="manipulator" />
        <param name="min_nonzero_move" type="double" value="0.005" />
+        <param name="joint_jump_tresh" type="double" value="45.0" />
+        <param name="default_frame_name" type="string" value= "/world"/>
        <rosparam command="load" file="$(find roboglue_ros)/config/jointDefaults.yaml" />
        <!-- ROS node loop parameters -->
+        <param name="startup_msg" type="string" value="STARTUP"/>
+        <param name="shutdown_msg" type="string" value="SHUTDOWN"/>
+        <param name="heartbeat_msg" type="string" value="HB"/>
        <param name="loop_rate" type="int" value="100" />
        <param name="msg_buffer" type="int" value="100" />
    </group>
@@ -44,9 +51,11 @@
        <param name="mqtt_INstate" type="string" value="roboglue_com/com2ros/state" />
        <param name="mqtt_INcommands" type="string" value="roboglue_com/com2ros/commands" />
        <param name="mqtt_INcoordinates" type="string" value="roboglue_com/com2ros/coordinates" />
+        <param name="mqtt_INinterface" type="string" value="roboglue_com/com2ros/interface" />
        <param name="mqtt_OUTstate" type="string" value="roboglue_com/ros2com/state" />
        <param name="mqtt_OUTcommands" type="string" value="roboglue_com/ros2com/commands" />
        <param name="mqtt_OUTcoordinates" type="string" value="roboglue_com/ros2com/coordinates" />
+        <param name="mqtt_OUTinterface" type="string" value="roboglue_com/ros2com/interface" />
    </node>

    <!-- Real Time Tracking -->
--- a/src/roboglue_ros/src/roboglue_broadcaster.cpp
+++ b/src/roboglue_ros/src/roboglue_broadcaster.cpp
@@ -0,0 +1,176 @@
+/*
+20191115 - Inizia la scrittura del nodo che si occupa di pubblicate le trasformazioni TF, documentazione su GIT
+*/
+
+////////// ROS INCLUDES /////////////
+#include "ros/ros.h"
+#include <roboglue_ros/roboglue_utils.h> /// utility function library
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <tf/transform_broadcaster.h>
+
+///////////////////////////////////////////
+///////// ROS CALLBACKS //////////////////
+/////////////////////////////////////////
+void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, publisherMap* pubs, positionData* pos, robotData* rdata) {
+    ROS_DEBUG("Received command: [%s]", msg->data.c_str());
+    try {
+        nlohmann::json c = nlohmann::json::parse(msg->data.c_str());
+        std::string command = c["command"];
+        nlohmann::json params = c["params"];
+        std::string action;
+        if (!command.compare("QUIT")){
+            is->isRunning = false;
+        } else if (!command.compare("SETFRAME")){
+            action = params["action"];
+            if (!action.compare("reset")){
+                pos->activeFrameName="world";
+                pos->activeFrame=geometry_msgs::Pose();
+            } else if (!action.compare("set")){
+                pos->activeFrameName = params["framename"];
+                geometry_msgs::TwistStamped tempFrame;
+                tempFrame.twist.linear.x = params["frame"]["x"].get<double>()/1000.0;
+                tempFrame.twist.linear.y = params["frame"]["y"].get<double>()/1000.0;
+                tempFrame.twist.linear.z = params["frame"]["z"].get<double>()/1000.0;
+                tempFrame.twist.angular.x = deg2rad(params["frame"]["rx"].get<double>());
+                tempFrame.twist.angular.y = deg2rad(params["frame"]["ry"].get<double>());
+                tempFrame.twist.angular.z = deg2rad(params["frame"]["rz"].get<double>());
+                pos->activeFrame = twist2Pose(tempFrame).pose;
+            } else;
+        } else {
+            ROS_WARN("Unknown command: %s", command.c_str());
+        }
+    } catch (nlohmann::json::exception &e){
+        ROS_ERROR("Failed to parse COMMAND: [%s]", e.what());
+    }
+}
+
+void stateCallback(const std_msgs::String::ConstPtr& msg, internalState* is, auxData* aux){
+    ROS_DEBUG("Received aux I/O State Update: [%s]", msg->data.c_str());
+}
+
+void interfaceCallback(const std_msgs::String::ConstPtr& msg, internalState *is){
+    ROS_DEBUG("Received Interface Message: [%s]", msg->data.c_str());
+}
+
+void heartBeatCallback(const ros::TimerEvent& t, publisherMap* p){
+    try {
+        std_msgs::String hbMsg;
+        hbMsg.data = std::string("HB:"+
+                                 boost::replace_all_copy(ros::this_node::getName(), "/", "")+
+                                 ":" + std::to_string(t.current_real.toSec()));
+        p->find("/roboglue_com/ros2com/interface")->second->publish(hbMsg);
+    } catch (std::exception &e) {
+        ROS_ERROR("Impossible to Send HeartBeat -> %s", e.what());
+    }
+}
+
+////////////////////////////////////////
+//////////////// MAIN //////////////////
+////////////////////////////////////////
+int main(int argc, char **argv) {
+    /// node variables ///
+    COMMON_parameters common_par;
+    /// internal state struct ////
+    internalState is;
+    positionData pos_data;
+    robotData robot_data;
+    auxData aux_data;
+    rvizData rviz_data;
+    int tfPubRate;
+    /// declare publisher map ///
+    publisherMap publishers;
+
+     /// setup node parameters ///
+    ros::init(argc, argv, "roboglue_broadcaster");
+    ros::NodeHandle nh;
+    /// set console log level ///
+    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug);
+
+    ///read parameters from server ///
+    nh.getParam("/roboglue_ros_broadcaster/parameter_ns", common_par.param_ns);
+    ROS_DEBUG("Using Parameter Nameserver [%s]", common_par.param_ns.c_str());
+    loadCommonParameters(&nh, &common_par);
+    // load robot model information
+    nh.getParam(common_par.param_ns+"robot_model_name", robot_data.robot_model_name);
+    nh.getParam(common_par.param_ns+"move_group_name", robot_data.move_group_name);
+    // load default frame parameters
+    nh.getParam(common_par.param_ns+"default_frame_name", pos_data.activeFrameName);
+    // load jog coord publish rate
+    nh.getParam("/roboglue_ros_broadcaster/tf_pub_rate", tfPubRate);
+
+    /// set spinner rate ///
+    ros::Rate loop_rate(common_par.loopRate);
+
+    ROS_INFO("Broadcaster Node Started");
+    ROS_DEBUG("Robot Model [%s] Loaded.. ", robot_data.robot_model_name.c_str());
+    ROS_DEBUG("Move Group  [%s] Loaded.. ", robot_data.move_group_name.c_str());
+
+    /// subscribe to incoming topics ///
+    ros::Subscriber command_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.commandSub, static_cast<uint32_t>(common_par.msgBufferSize),
+                                                                 boost::bind(commandCallback, _1, &is, &publishers, &pos_data, &robot_data));
+    ros::Subscriber state_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.stateSub, static_cast<uint32_t>(common_par.msgBufferSize),
+                                                               boost::bind(stateCallback, _1, &is, &aux_data));
+    /// advertise publish topics //
+    ros::Publisher command_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.commandPub,
+                                                                static_cast<uint32_t>(common_par.msgBufferSize));
+    ros::Publisher interface_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.interfacePub,
+                                                              static_cast<uint32_t>(common_par.msgBufferSize));
+
+    /// build a list of publisher to pass to mqtt callback ///
+    publishers[command_pub.getTopic()] = &command_pub;
+    publishers[interface_pub.getTopic()] = &interface_pub;
+
+    /// load the robot model for inverse kinematics and self collision checking ///
+    /// requires parameter server and moveIT to be active
+    moveit::planning_interface::MoveGroupInterface move_group(robot_data.move_group_name);
+    robot_data.kine_model = move_group.getRobotModel();
+    robot_data.kine_state = std::make_shared<robot_state::RobotState>(robot_data.kine_model);
+    robot_data.joint_modelGroup = robot_data.kine_state->getJointModelGroup(robot_data.move_group_name);
+    robot_data.joint_names = robot_data.joint_modelGroup->getVariableNames();
+    ROS_INFO("Planning in Frame: %s for link: %s", move_group.getPlanningFrame().c_str(), move_group.getEndEffectorLink().c_str());
+    ROS_INFO("IK calc in Frame: %s", robot_data.kine_model->getModelFrame().c_str());
+
+    /// create timed callbacks ///
+    ros::Timer heartBeat = nh.createTimer(ros::Duration(5.0),
+                                          boost::bind(heartBeatCallback, _1, &publishers));
+    /// variables for TF publication
+    tf::TransformBroadcaster tfBroadcaster;
+    tf::Transform planningFrame;
+    tf::Quaternion frameRot;
+    /// setup rviz palnned path visualization
+    rviz_data.vtools = std::make_shared<mvt::MoveItVisualTools>("world", "rviz_visual_tools", robot_data.kine_model);
+    ///////////////////// MAIN LOOP //////////////////////
+    bool startCycle = true;
+    while (ros::ok() && is.isRunning) {
+        if (startCycle){
+            startCycle = false;
+            declareStartup(&publishers, &common_par);
+            ros::spinOnce();
+        }
+        ROS_DEBUG_ONCE("Broadcaster Node Looping");
+        ros::spinOnce();
+
+        /// publish planning frame transform ///
+        /// verify execution frame and set to default if needed
+        ROS_DEBUG_ONCE("Planning Frame changed and publishing!");
+        planningFrame.setOrigin(tf::Vector3(pos_data.activeFrame.position.x,pos_data.activeFrame.position.y,pos_data.activeFrame.position.z));
+        frameRot.setRPY(pos_data.activeFrame.orientation.x,pos_data.activeFrame.orientation.y,pos_data.activeFrame.orientation.z);
+        planningFrame.setRotation(frameRot);
+        tfBroadcaster.sendTransform(tf::StampedTransform(planningFrame, ros::Time::now(), "world", "planning_frame"));
+        rviz_data.vtools->setBaseFrame("planning_frame");
+        move_group.setPoseReferenceFrame("planning_frame");
+
+        /// shutdown operations
+        if(ros::isShuttingDown()){
+            declareShutdown(&publishers, &common_par);
+            ros::spinOnce();
+        }
+        loop_rate.sleep();
+    }
+    ros::shutdown();
+    //////////////////////////////////////////////////////
+    return 0;
+}
+////////////////////////////////////////
+///////////// END MAIN /////////////////
+////////////////////////////////////////
--- a/src/roboglue_ros/src/roboglue_follower.cpp
+++ b/src/roboglue_ros/src/roboglue_follower.cpp
@@ -9,20 +9,24 @@
 20190308 - Inserita una funzione che si occupa di effettuare il blocco delle variaili.
 20190325 - Implementato uso del ros parameter server per la configurazione del nodo.
            Contiene i nomi delle code ros.
-20191023 - Passaggio dei commenti a GIT
+20191023 - Spostata Documentazione su GIT
 */
 ////////// ROS INCLUDES /////////////
 #include "ros/ros.h"
 #include <roboglue_ros/roboglue_utils.h> /// utility function library
 #include <moveit/move_group_interface/move_group_interface.h>

-
 ////////////////////////////////////////////
 ///////// FUNCTION DECLARATIONS ///////////
 //////////////////////////////////////////
 void testFunction( ros::Publisher* trj_pub);
 trajectory_msgs::JointTrajectory composeTrjMessage(std::vector<double> jtpos, std::vector<std::string> jnames);

+void sigintHandler(int sig, internalState* is){
+    ROS_DEBUG("Signal %d Received", sig);
+    if (is->isRunning) is->isRunning = false;
+    //ros::shutdown();
+}
 ///////////////////////////////////////////
 ///////// ROS CALLBACKS //////////////////
 /////////////////////////////////////////
@@ -64,8 +68,8 @@ void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, p
        } else {
            ROS_WARN("Unknown command: %s", command.c_str());
        }
-    } catch (nlohmann::json::exception){
-        ROS_ERROR("Failed to parse command!");
+    } catch (nlohmann::json::exception &e){
+        ROS_ERROR("Failed to parse COMMAND: [%s]", e.what());
    }
 }

@@ -113,6 +117,22 @@ void stateCallback(const std_msgs::String::ConstPtr& msg, internalState* is, aux
    ROS_DEBUG("Received aux I/O State Update: [%s]", msg->data.c_str());
 }

+void interfaceCallback(const std_msgs::String::ConstPtr& msg, internalState *is){
+    ROS_DEBUG("Received Interface Message: [%s]", msg->data.c_str());
+}
+
+void heartBeatCallback(const ros::TimerEvent& t, publisherMap* p){
+    try {
+        std_msgs::String hbMsg;
+        hbMsg.data = std::string("HB:"+
+                                 boost::replace_all_copy(ros::this_node::getName(), "/", "")+
+                                 ":" + std::to_string(t.current_real.toSec()));
+        p->find("/roboglue_com/ros2com/interface")->second->publish(hbMsg);
+    } catch (std::exception &e) {
+        ROS_ERROR("Impossible to Send HeartBeat -> %s", e.what());
+    }
+}
+
 void timedPublishCallback(const ros::TimerEvent&, internalState* is, publisherMap* pubs, positionData* pos, robotData* rdata){
    static geometry_msgs::TwistStamped lasttwPose;
    static trajectory_msgs::JointTrajectory lastjtPose;
@@ -176,62 +196,78 @@ void timedPublishCallback(const ros::TimerEvent&, internalState* is, publisherMa
 ////////////////////////////////////////
 int main(int argc, char **argv) {
    /// node variables ///
-    int loopRate, msgBufferSize, jogPubRate;
-    std::string param_ns;
-    ROS_topics ros_topics;
+    COMMON_parameters common_par;
    /// internal state struct ////
    internalState is;
    positionData pos_data;
    robotData robot_data;
    auxData aux_data;
    std::vector<std::string> joint_names;
-
+    int jogPubRate;
+    /// declare publisher map ////
+    publisherMap publishers;
    /// setup node parameters ///
    ros::init(argc, argv, "roboglue_follower");
+    //TODO: ros::init(argc, argv, "roboglue_follower", ros::init_options::NoSigintHandler);
    ros::NodeHandle nh;
-    ///read parameters from server ///
-    nh.getParam("/roboglue_ros_relay/parameter_ns", param_ns);
-    nh.getParam(param_ns+"loop_rate", loopRate);
-    nh.getParam(param_ns+"msg_buffer", msgBufferSize);
-    nh.getParam(param_ns+"INstate", ros_topics.stateSub);
-    nh.getParam(param_ns+"INcommands", ros_topics.commandSub);
-    nh.getParam(param_ns+"INcoordinates", ros_topics.coordSub);
-    nh.getParam(param_ns+"OUTstate", ros_topics.statePub);
-    nh.getParam(param_ns+"OUTcommands", ros_topics.commandPub);
-    nh.getParam(param_ns+"OUTcoordinates", ros_topics.coordPub);
-    nh.getParam(param_ns+"OUTdelta_jog", ros_topics.delta_jog);
-    nh.getParam(param_ns+"OUTdelta_joint_jog", ros_topics.delta_joint_jog);
-    nh.getParam(param_ns+"OUTtraj_jog", ros_topics.traj_jog);
-    nh.getParam(param_ns+"robot_model_name", robot_data.robot_model_name);
-    nh.getParam(param_ns+"move_group_name", robot_data.move_group_name);
-    nh.getParam(param_ns+"jointHomePosition", robot_data.joint_homes);
-    nh.getParam("/roboglue_ros_follower/jog_pub_rate", jogPubRate);
-
-    /// set spinner rate ///
-    ros::Rate loop_rate(loopRate);
    /// set console log level ///
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug);
-    ROS_INFO("Follower Node Started");
+    /// TODO: override default signal handler ///
+    // signal(SIGINT, boost::bind(sigintHandler, _1, &is));
+    ///read parameters from server ///
+    nh.getParam("/roboglue_ros_recorder/parameter_ns", common_par.param_ns);
+    ROS_DEBUG("Using Parameter Nameserver [%s]", common_par.param_ns.c_str());
+    loadCommonParameters(&nh, &common_par);
+    // load robot model information
+    nh.getParam(common_par.param_ns+"robot_model_name", robot_data.robot_model_name);
+    nh.getParam(common_par.param_ns+"move_group_name", robot_data.move_group_name);
+    nh.getParam(common_par.param_ns+"jointHomePosition", robot_data.joint_homes);
+    // load jog coord publish rate
+    nh.getParam("/roboglue_ros_follower/jog_pub_rate", jogPubRate);

+
+    /// set spinner rate ///
+    ros::Rate loop_rate(common_par.loopRate);
+
+    ROS_INFO("Follower Node Started");
+    ROS_DEBUG("Robot Model [%s] Loaded.. ", robot_data.robot_model_name.c_str());
+    ROS_DEBUG("Move Group [%s] Loaded.. ", robot_data.move_group_name.c_str());
+
+    /// subscribe to incoming topics ///
+    ros::Subscriber command_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.commandSub, static_cast<uint32_t>(common_par.msgBufferSize),
+                                                                 boost::bind(commandCallback, _1, &is, &publishers, &pos_data, &robot_data));
+    ros::Subscriber coordinate_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.coordSub, static_cast<uint32_t>(common_par.msgBufferSize),
+                                                                    boost::bind(coordinateCallback, _1, &publishers, &pos_data));
+    ros::Subscriber state_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.stateSub, static_cast<uint32_t>(common_par.msgBufferSize),
+                                                               boost::bind(stateCallback, _1, &is, &aux_data));
+    ros::Subscriber interface_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.interfaceSub, static_cast<uint32_t>(common_par.msgBufferSize),
+                                                               boost::bind(interfaceCallback, _1, &is));
    /// advertise publish topics ///
-    publisherMap publishers;
-    ros::Publisher jog_pub = nh.advertise<geometry_msgs::TwistStamped>(ros_topics.delta_jog,
-                                                                       static_cast<uint32_t>(msgBufferSize));
-    ros::Publisher jog_joint_pub = nh.advertise<geometry_msgs::TwistStamped>(ros_topics.delta_joint_jog,
-                                                                             static_cast<uint32_t>(msgBufferSize));
-    ros::Publisher trj_pub = nh.advertise<trajectory_msgs::JointTrajectory>(ros_topics.traj_jog,
-                                                                            static_cast<uint32_t>(msgBufferSize));
+    ros::Publisher jog_pub = nh.advertise<geometry_msgs::TwistStamped>(common_par.ros_topics.delta_jog,
+                                                                       static_cast<uint32_t>(common_par.msgBufferSize));
+    ros::Publisher jog_joint_pub = nh.advertise<geometry_msgs::TwistStamped>(common_par.ros_topics.delta_joint_jog,
+                                                                             static_cast<uint32_t>(common_par.msgBufferSize));
+    ros::Publisher trj_pub = nh.advertise<trajectory_msgs::JointTrajectory>(common_par.ros_topics.traj_jog,
+                                                                            static_cast<uint32_t>(common_par.msgBufferSize));
+    ros::Publisher command_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.commandPub,
+                                                                static_cast<uint32_t>(common_par.msgBufferSize));
+    ros::Publisher coord_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.coordPub,
+                                                              static_cast<uint32_t>(common_par.msgBufferSize));
+    ros::Publisher state_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.statePub,
+                                                              static_cast<uint32_t>(common_par.msgBufferSize));
+    ros::Publisher interface_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.interfacePub,
+                                                              static_cast<uint32_t>(common_par.msgBufferSize));
+
    /// build a list of publisher to pass to mqtt callback ///
+    publishers[command_pub.getTopic()] = &command_pub;
+    publishers[coord_pub.getTopic()] = &coord_pub;
+    publishers[state_pub.getTopic()] = &state_pub;
+    publishers[interface_pub.getTopic()] = &interface_pub;
+    /// publishers to jog command interfaces
    publishers["jog_pub"] = &jog_pub;
    publishers["jog_joint_pub"] = &jog_joint_pub;
    publishers["trj_pub"] = &trj_pub;
-    /// subscribe to incoming topics ///
-    ros::Subscriber command_sub = nh.subscribe<std_msgs::String>(ros_topics.commandSub, static_cast<uint32_t>(msgBufferSize),
-                                                                 boost::bind(commandCallback, _1, &is, &publishers, &pos_data, &robot_data));
-    ros::Subscriber coordinate_sub = nh.subscribe<std_msgs::String>(ros_topics.coordSub, static_cast<uint32_t>(msgBufferSize),
-                                                                    boost::bind(coordinateCallback, _1, &publishers, &pos_data));
-    ros::Subscriber state_sub = nh.subscribe<std_msgs::String>(ros_topics.stateSub, static_cast<uint32_t>(msgBufferSize),
-                                                               boost::bind(stateCallback, _1, &is, &aux_data));
+
    /// load the robot model for inverse kinematics and self collision checking ///
    /// requires parameter server and moveIT to be active
    moveit::planning_interface::MoveGroupInterface move_group(robot_data.move_group_name);
@@ -245,15 +281,27 @@ int main(int argc, char **argv) {
    /// create timed callbacks ///
    ros::Timer timedPublish = nh.createTimer(ros::Duration(1.0/static_cast<double>(jogPubRate)),
                                             boost::bind(timedPublishCallback, _1, &is, &publishers, &pos_data, &robot_data));
-    // now we can get inverse kinematics from kinematic state using the joint model
+    ros::Timer heartBeat = nh.createTimer(ros::Duration(5.0),
+                                         boost::bind(heartBeatCallback, _1, &publishers));

+    ///////////////////// MAIN LOOP //////////////////////
+    bool startCycle = true;
    while (ros::ok() && is.isRunning) {
-        ROS_DEBUG_ONCE("Follower Node Looping");
-        //testFunction(&trj_pub);
+        if (startCycle){
+            startCycle = false;
+            declareStartup(&publishers, &common_par);
            ros::spinOnce();
-        //if (is.isRealtime)
+        }
+        ROS_DEBUG_ONCE("Follower Node Looping");
+        ros::spinOnce();
+        if(ros::isShuttingDown()){
+            declareShutdown(&publishers, &common_par);
+            ros::spinOnce();
+        }
        loop_rate.sleep();
    }
+    ros::shutdown();
+    //////////////////////////////////////////////////////
    return 0;
 }
 ////////////////////////////////////////
--- a/src/roboglue_ros/src/roboglue_recorder.cpp
+++ b/src/roboglue_ros/src/roboglue_recorder.cpp
@@ -33,31 +33,20 @@
            che non si incappi in configurazioni instabili.
 20190708 - Implementazione funzioni di visualizzazione del percorso e pulizia dello schermo prima
            dell'esecuzione del file.
+20191029 - Spostata la documentazione su GIT
 */
 ////////// ROS INCLUDES /////////////
 #include "ros/ros.h"
 #include <roboglue_ros/roboglue_utils.h> /// utility function library
 #include <moveit/move_group_interface/move_group_interface.h>
+#include <tf/transform_broadcaster.h>
 ///////// STANDARD LIBRARIES ////////
 #include <iostream>
 #include <fstream>

-typedef struct {
-    nlohmann::json* metaFile = nullptr;
-    rapidcsv::Document* dataFile = nullptr;
-    std::string meta_dir_name;
-    std::string data_dir_name;
-    std::string meta_template_name;
-    std::string data_template_name;
-    std::string meta_ext;
-    std::string data_ext;
-    std::vector<pointRecord>* recordVect;
-    std::vector<pointRecord>* playVect;
-} fileData;
-
-// posizione degli elementi nella riga del csv
-enum VPOS {X,Y,Z,RX,RY,RZ,DT,DS,VMAX};
-
+////////////////////////////////////////////
+///////// FUNCTION DECLARATIONS ///////////
+//////////////////////////////////////////
 bool openFile(internalState* is, fileData* fd, nlohmann::json meta);
 bool closeFile(internalState* is, fileData* fd);

@@ -120,11 +109,18 @@ void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, p
                is->isPlaying = false;
                is->isRecording = false;
            }
+        } else if (!command.compare("SETFRAME")) {
+            action = params["action"];
+            if (!action.compare("reset")){
+                is->setFrame = true;
+            } else if (!action.compare("set")){
+                is->setFrame = true;
+            }
        } else {
            ROS_WARN("Unknown command: %s", command.c_str());
        }
    } catch (nlohmann::json::exception &e) {
-        ROS_ERROR("Failed to parse command: [%s]", e.what());
+        ROS_ERROR("Failed to parse COMMAND: [%s]", e.what());
        is->isPlaying = false;
        is->isRealtime = false;
        is->isRecording = false;
@@ -180,6 +176,7 @@ void coordinateCallback(const std_msgs::String::ConstPtr& msg, internalState* is

 void stateCallback(const std_msgs::String::ConstPtr& msg, internalState*is, auxData* aux){
    ROS_DEBUG("Received aux I/O State Update: [%s]", msg->data.c_str());
+    try {
        nlohmann::json auxParsed = nlohmann::json::parse(msg->data);
        nlohmann::json digitalParsed = auxParsed["digital"];
        nlohmann::json analogParsed = auxParsed["analog"];
@@ -197,6 +194,25 @@ void stateCallback(const std_msgs::String::ConstPtr& msg, internalState*is, auxD
            aux->digital.assign(tempDigital.begin(), tempDigital.end());
            aux->analog.assign(tempAnalog.begin(), tempAnalog.end());
        }
+    } catch (nlohmann::json::exception &e) {
+         ROS_ERROR("Failed to parse STATE: [%s]", e.what());
+    }
+}
+
+void interfaceCallback(const std_msgs::String::ConstPtr& msg, internalState *is){
+    ROS_DEBUG("Received Interface Message: [%s]", msg->data.c_str());
+}
+
+void heartBeatCallback(const ros::TimerEvent& t, publisherMap* p){
+    try {
+        std_msgs::String hbMsg;
+        hbMsg.data = std::string("HB:"+
+                                 boost::replace_all_copy(ros::this_node::getName(), "/", "")+
+                                 ":" + std::to_string(t.current_real.toSec()));
+        p->find("/roboglue_com/ros2com/interface")->second->publish(hbMsg);
+    } catch (std::exception &e) {
+        ROS_ERROR("Impossible to Send HeartBeat -> %s", e.what());
+    }
 }

 ////////////////////////////////////////
@@ -204,10 +220,7 @@ void stateCallback(const std_msgs::String::ConstPtr& msg, internalState*is, auxD
 ////////////////////////////////////////
 int main(int argc, char **argv) {
    /// node variables ////
-    int loopRate, msgBufferSize;
-    double nonzero_move;
-    std::string param_ns;
-    ROS_topics ros_topics;
+    COMMON_parameters common_par;
    /// internal node state variables ///
    internalState is;
    auxData aux_data;
@@ -221,6 +234,9 @@ int main(int argc, char **argv) {
    std::vector<pointRecord> playVector;
    file_data.recordVect = &recordVector;
    file_data.playVect = &playVector;
+    double nonzero_move;
+    /// declare publisher map ///
+    publisherMap publishers;

    /// player variables ///
    double dsCounter, dtCounter;
@@ -231,20 +247,24 @@ int main(int argc, char **argv) {
    /// setup node parameters ///
    ros::init(argc, argv, "roboglue_recorder");
    ros::NodeHandle nh;    
+    /// set console log level ///
+    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug);
+
    /// read parameters from server ///
    //load common parameters
-    nh.getParam("/roboglue_ros_recorder/parameter_ns", param_ns);
-    nh.getParam(param_ns+"loop_rate", loopRate);
-    nh.getParam(param_ns+"msg_buffer", msgBufferSize);
-    nh.getParam(param_ns+"min_nonzero_move", nonzero_move);
-    nh.getParam(param_ns+"INstate", ros_topics.stateSub);
-    nh.getParam(param_ns+"INcommands", ros_topics.commandSub);
-    nh.getParam(param_ns+"INcoordinates", ros_topics.coordSub);
-    nh.getParam(param_ns+"OUTstate", ros_topics.statePub);
-    nh.getParam(param_ns+"OUTcommands", ros_topics.commandPub);
-    nh.getParam(param_ns+"OUTcoordinates", ros_topics.coordPub);
+    nh.getParam("/roboglue_ros_recorder/parameter_ns", common_par.param_ns);
+    ROS_DEBUG("Using Parameter Nameserver [%s]", common_par.param_ns.c_str());
+    loadCommonParameters(&nh, &common_par);
+    // TODO: inserire anche questi parametri tra quelli comuni
+    // load robot model information
+    nh.getParam(common_par.param_ns+"min_nonzero_move", nonzero_move);
+    nh.getParam(common_par.param_ns+"robot_model_name", robot_data.robot_model_name);
+    nh.getParam(common_par.param_ns+"move_group_name", robot_data.move_group_name);
+    nh.getParam(common_par.param_ns+"joint_jump_tresh", robot_data.joint_jump_tresh);
+    // load default frame parameters
+    nh.getParam(common_par.param_ns+"default_frame_name", pos_data.activeFrameName);
    // load spin rate
-    nh.getParam("/roboglue_ros_recorder/loop_rate", loopRate);
+    nh.getParam("/roboglue_ros_recorder/loop_rate", common_par.loopRate);
    //load meta template filename
    nh.getParam("/roboglue_ros_recorder/meta_dir", file_data.meta_dir_name);
    nh.getParam("/roboglue_ros_recorder/meta_template", file_data.meta_template_name);
@@ -257,36 +277,39 @@ int main(int argc, char **argv) {
    // load coordinate send operation mode (time/distance)
    nh.getParam("/roboglue_ros_recorder/point_group_mode", robot_data.point_group_mode);
    nh.getParam("/roboglue_ros_recorder/planning_mode", robot_data.planning_mode);
-    // load robot model information
-    nh.getParam(param_ns+"robot_model_name", robot_data.robot_model_name);
-    nh.getParam(param_ns+"move_group_name", robot_data.move_group_name);

-    /// set console log level ///
-    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Warn);
    /// set spinner rate ///
-    ros::Rate loop_rate(loopRate);
-
+    ros::Rate loop_rate(common_par.loopRate);
    ROS_INFO("Recorder Node Started");

-    ros::Subscriber command_sub = nh.subscribe<std_msgs::String>(ros_topics.commandSub, static_cast<uint32_t>(msgBufferSize),
+    /// subscribe to incoming topics ///
+    ros::Subscriber command_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.commandSub, static_cast<uint32_t>(common_par.msgBufferSize),
                                                                 boost::bind(commandCallback, _1, &is, &pos_data, &rviz_data, &file_data));
-    ros::Subscriber coordinate_sub = nh.subscribe<std_msgs::String>(ros_topics.coordSub, static_cast<uint32_t>(msgBufferSize),
+    ros::Subscriber coordinate_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.coordSub, static_cast<uint32_t>(common_par.msgBufferSize),
                                                                    boost::bind(coordinateCallback, _1, &is, &pos_data));
-    ros::Subscriber state_sub = nh.subscribe<std_msgs::String>(ros_topics.stateSub, static_cast<uint32_t>(msgBufferSize),
+    ros::Subscriber state_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.stateSub, static_cast<uint32_t>(common_par.msgBufferSize),
                                                               boost::bind(stateCallback, _1, &is, &aux_data));
+    ros::Subscriber interface_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.interfaceSub, static_cast<uint32_t>(common_par.msgBufferSize),
+                                                               boost::bind(interfaceCallback, _1, &is));
    /// advertise publish topics ///
-    publisherMap publishers;
-    ros::Publisher command_pub = nh.advertise<std_msgs::String>(ros_topics.commandPub,
-                                                                static_cast<uint32_t>(msgBufferSize));
-    ros::Publisher coord_pub = nh.advertise<std_msgs::String>(ros_topics.coordPub,
-                                                              static_cast<uint32_t>(msgBufferSize));
-    ros::Publisher state_pub = nh.advertise<std_msgs::String>(ros_topics.statePub,
-                                                              static_cast<uint32_t>(msgBufferSize));
+    ros::Publisher command_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.commandPub,
+                                                                static_cast<uint32_t>(common_par.msgBufferSize));
+    ros::Publisher coord_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.coordPub,
+                                                              static_cast<uint32_t>(common_par.msgBufferSize));
+    ros::Publisher state_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.statePub,
+                                                              static_cast<uint32_t>(common_par.msgBufferSize));
+    ros::Publisher interface_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.interfacePub,
+                                                              static_cast<uint32_t>(common_par.msgBufferSize));

    /// build a list of publisher to pass to mqtt callback ///
    publishers[command_pub.getTopic()] = &command_pub;
    publishers[coord_pub.getTopic()] = &coord_pub;
    publishers[state_pub.getTopic()] = &state_pub;
+    publishers[interface_pub.getTopic()] = &interface_pub;
+
+    /// create timed callbacks ///
+    ros::Timer heartBeat = nh.createTimer(ros::Duration(5.0),
+                                         boost::bind(heartBeatCallback, _1, &publishers));

    /// load the robot model for inverse kinematics and self collision checking ///
    /// requires parameter server and moveIT to be active
@@ -295,11 +318,12 @@ int main(int argc, char **argv) {
    robot_data.kine_state = std::make_shared<robot_state::RobotState>(robot_data.kine_model);
    robot_data.joint_modelGroup = robot_data.kine_state->getJointModelGroup(robot_data.move_group_name);
    robot_data.joint_names = robot_data.joint_modelGroup->getVariableNames();
+    move_group.setPoseReferenceFrame(pos_data.activeFrameName);
    ROS_INFO("Planning in Frame: %s for link: %s", move_group.getPlanningFrame().c_str(), move_group.getEndEffectorLink().c_str());
    ROS_INFO("IK calc in Frame: %s", robot_data.kine_model->getModelFrame().c_str());

    /// setup rviz palnned path visualization
-    rviz_data.vtools = std::make_shared<mvt::MoveItVisualTools>("base_link", "rviz_visual_tools", robot_data.kine_model);
+    rviz_data.vtools = std::make_shared<mvt::MoveItVisualTools>("world", "rviz_visual_tools", robot_data.kine_model);

    bool startCycle = true;
    while (ros::ok() && is.isRunning){
@@ -309,11 +333,24 @@ int main(int argc, char **argv) {
            rviz_data.vtools->loadRemoteControl();
            rviz_data.vtools->deleteAllMarkers();
            rviz_data.vtools->trigger();
-        }
+            declareStartup(&publishers, &common_par);
            ros::spinOnce();
+        }
+        ROS_DEBUG_ONCE("Recorder Node Looping");
+        ros::spinOnce();
+        if(ros::isShuttingDown()){
+            declareShutdown(&publishers, &common_par);
+            ros::spinOnce();
+        }
          /////////////////////////////////////////
         /////////// PLAY FILE ///////////////////
        /////////////////////////////////////////
+        if (is.setFrame){
+            rviz_data.vtools->setBaseFrame("planning_frame");
+            move_group.setPoseReferenceFrame("planning_frame");
+            rviz_data.vtools->trigger();
+            is.setFrame = false;
+        }
        if (is.isPlaying){    
            // i -> row counter for temp vector planning in both cartesian and joint mode
            ROS_INFO_COND(i==0,"Start Play file: [%s]", file_data.metaFile->at("name").get<std::string>().c_str());
@@ -344,9 +381,12 @@ int main(int argc, char **argv) {
                tempPlanningVector.clear();
                for (auto cc : tempPlayVector) {
                    tempPlanningVector.push_back(twist2Pose(cc.point));
-                    tempPlanningVector.back().header.frame_id="/world";
+                    tempPlanningVector.back().header.frame_id=pos_data.activeFrameName;
                }
                //TODO sincronizzazione delle uscite ausiliarie con il movimento del robot!!!
+                //TODO l'invio di traiettorie segmentato in più esecuzioni funziona
+                // si potrebbe quindi sincronizzare l'attivazione delle uscite ausiliarie ad ogni invio di nuova traiettoria
+
                // select right planner
                if (!robot_data.planning_mode.compare("path")){
                    const double jmp_thr= 0.00;
@@ -362,17 +402,18 @@ int main(int argc, char **argv) {
                    move_group.setStartStateToCurrentState();
                    move_group.computeCartesianPath(pss,ee_step,jmp_thr,traj, false, &ec);
                    ROS_DEBUG("Trajectory Compute ExitCode: %d",ec.val);
-//                    BUG il controllo del salto dei giunti crasha il nodo.
-//                    if (checkJointJump(boost::shared_ptr<moveit_msgs::RobotTrajectory>(&traj), 90.0)){
-//                        ROS_WARN("Joint Jump Detected!!!");
-//                    }
+                    // check for joint jump and if not detected execute motion
+                    if (checkJointJump(&traj, robot_data.joint_jump_tresh)){
+                        ROS_WARN("Joint Jump Detected!!! \n ABORTING EXECUTION ");
+                    } else {
                        jointPlan.trajectory_ = traj;
                        plotTrajectory(&rviz_data, &robot_data, &traj);
                        // async execute planned trajectory
                        moveit::planning_interface::MoveItErrorCode retval = move_group.execute(jointPlan);
                        ROS_INFO("Finished executing first [%u] datapoints, execute: %d", i, retval.val);
+                    }
                } else if (!robot_data.planning_mode.compare("joint")){
-                    // TODO SCRIVERE LA LOGICA PER IL PERCORSO JOINT QUANDO
+                    // TODO: SCRIVERE LA LOGICA PER IL PERCORSO JOINT QUANDO
                    // QUANDO I PROBLEMI DI STABILITÀ DEL MOVIMENTO SONO RISOLTI
                } else {
                    ROS_ERROR("Invalid Plannning Mode!");
@@ -421,7 +462,7 @@ int main(int argc, char **argv) {
 ////////////////////////////////////////

 bool openFile(internalState* is, fileData* fd, nlohmann::json meta){
-    if (!is->isFileOpen){
+    if (!is->isFileOpen && !meta["name"].get<std::string>().empty()){
        // open metafile by name
        std::string metaInFilename = fd->meta_dir_name + meta["name"].get<std::string>() + fd->meta_ext;
        ROS_DEBUG("Opening Meta File: [%s]", metaInFilename.c_str());
@@ -489,6 +530,7 @@ bool openFile(internalState* is, fileData* fd, nlohmann::json meta){
            return false;
        }
    }
+    return false;
 }

 bool closeFile(internalState* is, fileData* fd){
@@ -503,7 +545,7 @@ bool closeFile(internalState* is, fileData* fd){
        }
       fd->playVect->clear();
       fd->recordVect->clear();
-    }
+    } return true;
 }

 bool startRecord(internalState* is, fileData* fd, nlohmann::json meta){
@@ -645,6 +687,7 @@ bool startPlay(internalState* is, fileData* fd, nlohmann::json meta){
        return openFile(is,fd,meta);
    else if (!is->isPlaying && is->isFileOpen)
        return true;
+    else return false;
 }

 bool stopPlay(internalState* is, fileData* fd){
@@ -655,7 +698,7 @@ bool stopPlay(internalState* is, fileData* fd){
 }

 void plotPoints (rvizData* rvdata, fileData* fd){
-    if (fd->playVect != nullptr){
+    if (fd->playVect != nullptr && fd->playVect->size()>0) {
        for(auto point : *fd->playVect){
            rvdata->vtools->publishSphere(twist2Pose(point.point).pose, rvt::RED, rvt::MEDIUM);
        }
@@ -668,7 +711,10 @@ void plotPoints (rvizData* rvdata, fileData* fd){
 }

 void plotTrajectory(rvizData* rvdata, robotData* robotdata, moveit_msgs::RobotTrajectory* traj) {
+    rvdata->vtools->setBaseFrame("world");
+    rvdata->vtools->trigger();
    rvdata->vtools->publishTrajectoryLine(*traj, robotdata->joint_modelGroup, rvt::colors::BLUE);
+    rvdata->vtools->setBaseFrame("planning_frame");
    rvdata->vtools->trigger();
 };

--- a/src/roboglue_ros/src/roboglue_relay.cpp
+++ b/src/roboglue_ros/src/roboglue_relay.cpp
@@ -6,6 +6,7 @@
            modi che li devono interpretare. qui viene trattato tutto come stringa.
 20190325 - Implementato uso del ros parameter server per la configurazione del nodo.
            Contiene i nomi delle code sia ros che mqtt.
+20191028 - Documentazione Spostata su GIT
 */

 ////////// ROS INCLUDES /////////////
@@ -37,21 +38,23 @@ class mqtt2rosClass: public virtual mqtt::callback {
             ROS_DEBUG("MessageDelivered, t:%d",tok->get_message_id());
        }
        void message_arrived(mqtt::const_message_ptr msg) override{
-            ROS_DEBUG("MqttMessage!");
            std::string topic(msg->get_topic());
            std::string payload(msg->get_payload_str());
            std_msgs::String rosMsg;
            rosMsg.data = payload;

            if (!topic.compare(mqtt_data_.MQTTcommandSub)){
-                ROS_DEBUG("Command! -> %s", ros_data_.commandSub.c_str());
+                ROS_DEBUG("MQTT Message: Command! -> %s", ros_data_.commandSub.c_str());
                pubs_[ros_data_.commandSub]->publish(rosMsg);
            } else if (!topic.compare(mqtt_data_.MQTTcoordSub)){
-                ROS_DEBUG("Coordinate! -> %s", ros_data_.coordSub.c_str());
+                ROS_DEBUG("MQTT Message: Coordinate! -> %s", ros_data_.coordSub.c_str());
                pubs_[ros_data_.coordSub]->publish(rosMsg);
            } else if (!topic.compare(mqtt_data_.MQTTstateSub)){
-                ROS_DEBUG("State! -> %s", ros_data_.stateSub.c_str());
+                ROS_DEBUG("MQTT Message: State! -> %s", ros_data_.stateSub.c_str());
                pubs_[ros_data_.stateSub]->publish(rosMsg);
+            } else if (!topic.compare(mqtt_data_.MQTTinterfaceSub)) {
+                ROS_DEBUG("MQTT Message: Interface! -> %s", ros_data_.interfaceSub.c_str());
+                pubs_[ros_data_.interfaceSub]->publish(rosMsg);
            } else {
                ROS_ERROR("Topic NOT Implemented");
            }
@@ -62,51 +65,61 @@ class mqtt2rosClass: public virtual mqtt::callback {
 /////////////// ROS CALLBACKS //////////
 ////////////////////////////////////////
 void commandCallback(const std_msgs::String::ConstPtr msg, mqtt::async_client* cli_, MQTT_topics* mqtttopics){
-    ROS_DEBUG("RosMessage: Command!");
+    ROS_DEBUG("ROS Message: Command! -> %s", msg->data.c_str());
    cli_->publish(mqtt::make_message(mqtttopics->MQTTcommandPub, msg->data.c_str()));
 }
 void poseCallback(const std_msgs::String::ConstPtr msg, mqtt::async_client* cli_, MQTT_topics* mqtttopics){
-    ROS_DEBUG("RosMessage: Pose!");
+    ROS_DEBUG("ROS Message: Pose! -> %s", msg->data.c_str());
    cli_->publish(mqtt::make_message(mqtttopics->MQTTcoordPub, msg->data.c_str()));
 }
 void stateCallback(const std_msgs::String::ConstPtr msg, mqtt::async_client* cli_, MQTT_topics* mqtttopics){
-    ROS_DEBUG("RosMessage: State!");
+    ROS_DEBUG("ROS Message: State! -> %s", msg->data.c_str());
    cli_->publish(mqtt::make_message(mqtttopics->MQTTstatePub, msg->data.c_str()));
 }
+void interfaceCallback(const std_msgs::String::ConstPtr msg, mqtt::async_client* cli_, MQTT_topics* mqtttopics){
+   ROS_DEBUG("ROS Message: Interface! -> %s", msg->data.c_str());
+   cli_->publish(mqtt::make_message(mqtttopics->MQTTinterfacePub, msg->data.c_str()));
+}
+void heartBeatCallback(const ros::TimerEvent& t, publisherMap* p){
+    std_msgs::String hbMsg;
+    try {
+        std_msgs::String hbMsg;
+        hbMsg.data = std::string("HB:"+
+                                 boost::replace_all_copy(ros::this_node::getName(), "/", "")+
+                                 ":" + std::to_string(t.current_real.toSec()));
+        p->find("/roboglue_com/ros2com/interface")->second->publish(hbMsg);
+    } catch (std::exception &e) {
+        ROS_ERROR("Impossible to Send HeartBeat -> %s", e.what());
+    }
+}

 ////////////////////////////////////////
 //////////////// MAIN //////////////////
 ////////////////////////////////////////
 int main(int argc, char **argv) {
    /// node variables ///
-    int loopRate, msgBufferSize;
-    std::string param_ns;
+
+    COMMON_parameters common_par;
    MQTT_topics mqtt_topics;
-    ROS_topics ros_topics;
    /// setup node parameters ///
    ros::init(argc, argv, "roboglue_relay");
    ros::NodeHandle nh;
    ///read parameters from server ///
-    nh.getParam("/roboglue_ros_relay/parameter_ns", param_ns);
-    nh.getParam(param_ns+"loop_rate", loopRate);
-    nh.getParam(param_ns+"msg_buffer", msgBufferSize);
-    nh.getParam(param_ns+"INstate", ros_topics.stateSub);
-    nh.getParam(param_ns+"INcommands", ros_topics.commandSub);
-    nh.getParam(param_ns+"INcoordinates", ros_topics.coordSub);
-    nh.getParam(param_ns+"OUTstate", ros_topics.statePub);
-    nh.getParam(param_ns+"OUTcommands", ros_topics.commandPub);
-    nh.getParam(param_ns+"OUTcoordinates", ros_topics.coordPub);
+    nh.getParam("/roboglue_ros_relay/parameter_ns", common_par.param_ns);
+    loadCommonParameters(&nh, &common_par);
    nh.getParam("/roboglue_ros_relay/mqtt_INstate", mqtt_topics.MQTTstateSub);
    nh.getParam("/roboglue_ros_relay/mqtt_INcommands", mqtt_topics.MQTTcommandSub);
    nh.getParam("/roboglue_ros_relay/mqtt_INcoordinates", mqtt_topics.MQTTcoordSub);
+    nh.getParam("/roboglue_ros_relay/mqtt_INinterface", mqtt_topics.MQTTinterfaceSub);
    nh.getParam("/roboglue_ros_relay/mqtt_OUTstate", mqtt_topics.MQTTstatePub);
    nh.getParam("/roboglue_ros_relay/mqtt_OUTcommands", mqtt_topics.MQTTcommandPub);
    nh.getParam("/roboglue_ros_relay/mqtt_OUTcoordinates", mqtt_topics.MQTTcoordPub);
+    nh.getParam("/roboglue_ros_relay/mqtt_OUTinterface", mqtt_topics.MQTTinterfacePub);
    nh.getParam("/roboglue_ros_relay/mqtt_host", mqtt_topics.mqttHost);
    nh.getParam("/roboglue_ros_relay/mqtt_qos", mqtt_topics.mqttQoS);

    /// set spinner rate ///
-    ros::Rate loop_rate(loopRate);
+    ros::Rate loop_rate(common_par.loopRate);
    /// set console log level ///
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Warn);
    ROS_INFO("Relay Node Started");
@@ -115,24 +128,34 @@ int main(int argc, char **argv) {

    /// advertise publish topics ///
    publisherMap publishers;
-    ros::Publisher command_pub = nh.advertise<std_msgs::String>(ros_topics.commandSub,static_cast<uint32_t>(msgBufferSize));
-    ros::Publisher pose_pub = nh.advertise<std_msgs::String>(ros_topics.coordSub, static_cast<uint32_t>(msgBufferSize));
-    ros::Publisher state_pub = nh.advertise<std_msgs::String>(ros_topics.stateSub,static_cast<uint32_t>(msgBufferSize));
+    ros::Publisher command_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.commandSub,static_cast<uint32_t>(common_par.msgBufferSize));
+    ros::Publisher pose_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.coordSub, static_cast<uint32_t>(common_par.msgBufferSize));
+    ros::Publisher state_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.stateSub,static_cast<uint32_t>(common_par.msgBufferSize));
+    ros::Publisher interface_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.interfaceSub,static_cast<uint32_t>(common_par.msgBufferSize));
+    ros::Publisher interface_pub2 = nh.advertise<std_msgs::String>(common_par.ros_topics.interfacePub,static_cast<uint32_t>(common_par.msgBufferSize));
+
    /// build a list of publisher to pass to mqtt callback ///
    publishers[command_pub.getTopic()] = &command_pub;
    publishers[pose_pub.getTopic()] = &pose_pub;
    publishers[state_pub.getTopic()] = &state_pub;
+    publishers[interface_pub.getTopic()] = &interface_pub;
+    publishers[interface_pub2.getTopic()] = &interface_pub2;
    /// create mqtt callback ///
-    mqtt2rosClass mqtt2ros(client, publishers, mqtt_topics, ros_topics);
+    mqtt2rosClass mqtt2ros(client, publishers, mqtt_topics, common_par.ros_topics);

    /// subscribe to incoming topics ///
-    ros::Subscriber command_sub = nh.subscribe<std_msgs::String>(ros_topics.commandPub,static_cast<uint32_t>(msgBufferSize),
+    ros::Subscriber command_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.commandPub,static_cast<uint32_t>(common_par.msgBufferSize),
                                                                 boost::bind(commandCallback, _1, &client, &mqtt_topics));
-    ros::Subscriber pose_sub = nh.subscribe<std_msgs::String>(ros_topics.coordPub,static_cast<uint32_t>(msgBufferSize),
+    ros::Subscriber pose_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.coordPub,static_cast<uint32_t>(common_par.msgBufferSize),
                                                              boost::bind(poseCallback, _1, &client, &mqtt_topics));
-    ros::Subscriber state_sub = nh.subscribe<std_msgs::String>(ros_topics.statePub,static_cast<uint32_t>(msgBufferSize),
+    ros::Subscriber state_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.statePub,static_cast<uint32_t>(common_par.msgBufferSize),
                                                           boost::bind(stateCallback, _1, &client, &mqtt_topics));
+    ros::Subscriber interface_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.interfacePub,static_cast<uint32_t>(common_par.msgBufferSize),
+                                                              boost::bind(interfaceCallback, _1, &client, &mqtt_topics));

+    /// create timed callbacks ///
+    ros::Timer heartBeat = nh.createTimer(ros::Duration(5.0),
+                                         boost::bind(heartBeatCallback, _1, &publishers));
     /// CONNECT MQTT CLIENT ///
    ROS_INFO("Connecting to MQTT...");
    try {
@@ -141,18 +164,29 @@ int main(int argc, char **argv) {
        client.subscribe(mqtt_topics.MQTTcommandSub,mqtt_topics.mqttQoS);
        client.subscribe(mqtt_topics.MQTTcoordSub,mqtt_topics.mqttQoS);
        client.subscribe(mqtt_topics.MQTTstateSub,mqtt_topics.mqttQoS);
+        client.subscribe(mqtt_topics.MQTTinterfaceSub,mqtt_topics.mqttQoS);
        client.start_consuming();
    } catch (mqtt::exception &e) {
        ROS_ERROR("Failed to Connect to MQTT: [%s]", e.what());
        ros::shutdown();
    }
-
+    ////////////// MAIN LOOP /////////////
+    bool startCycle = true;
    while (ros::ok()) {
+        if (startCycle){
+            startCycle = false;
+            declareStartup(&publishers, &common_par);
+            ros::spinOnce();
+        }
        ROS_DEBUG_ONCE("Relay Node Looping");
        ros::spinOnce();
+        if(ros::isShuttingDown()){
+            declareShutdown(&publishers, &common_par);
+            ros::spinOnce();
+        }
        loop_rate.sleep();
    }
-
+    ///////////////////////////////////////
    try {
        client.stop_consuming();
        client.disable_callbacks();
@@ -160,6 +194,5 @@ int main(int argc, char **argv) {
    } catch (mqtt::exception &e) {
        ROS_DEBUG("Error Disconnecting MQTT: [%s]", e.what());
    }
-
    return 0;
 }
--- a/src/roboglue_ros/src/roboglue_utils.cpp
+++ b/src/roboglue_ros/src/roboglue_utils.cpp
@@ -1,6 +1,50 @@
 #include <roboglue_ros/roboglue_utils.h>

 /// Utilities funtion used in roboglue ros nodes ///
+///
+////////////////////////////////////////////////////
+/////////////// NODE MANAGEMENT ///////////////////
+//////////////////////////////////////////////////
+bool loadCommonParameters(ros::NodeHandle* nh, COMMON_parameters *p) {
+    ROS_DEBUG("Node [%s] loading common parameters", ros::this_node::getName().c_str());
+    nh->getParam(p->param_ns+"INstate", p->ros_topics.stateSub);
+    nh->getParam(p->param_ns+"INcommands", p->ros_topics.commandSub);
+    nh->getParam(p->param_ns+"INcoordinates", p->ros_topics.coordSub);
+    nh->getParam(p->param_ns+"INinterface", p->ros_topics.interfaceSub);
+    nh->getParam(p->param_ns+"OUTstate", p->ros_topics.statePub);
+    nh->getParam(p->param_ns+"OUTcommands", p->ros_topics.commandPub);
+    nh->getParam(p->param_ns+"OUTcoordinates", p->ros_topics.coordPub);
+    nh->getParam(p->param_ns+"OUTinterface", p->ros_topics.interfacePub);
+    nh->getParam(p->param_ns+"OUTdelta_jog", p->ros_topics.delta_jog);
+    nh->getParam(p->param_ns+"OUTdelta_joint_jog", p->ros_topics.delta_joint_jog);
+    nh->getParam(p->param_ns+"OUTtraj_jog", p->ros_topics.traj_jog);
+    nh->getParam(p->param_ns+"loop_rate", p->loopRate);
+    nh->getParam(p->param_ns+"msg_buffer", p->msgBufferSize);
+    nh->getParam(p->param_ns+"startup_msg", p->startupMsg.data);
+    nh->getParam(p->param_ns+"shutdown_msg", p->shutdownMsg.data);
+    return true;
+}
+
+void declareStartup(publisherMap* publishers, COMMON_parameters* common_par){
+    common_par->startupMsg.data = common_par->startupMsg.data + ":" +
+                                    boost::replace_all_copy(ros::this_node::getName(), "/", "") +
+                                    ":" + std::to_string(ros::Time::now().toSec());
+    ROS_DEBUG("StartupMessage: %s", common_par->startupMsg.data.c_str());
+    publishers->find(common_par->ros_topics.interfacePub)->second->publish(common_par->startupMsg);
+}
+
+void declareShutdown(publisherMap* publishers, COMMON_parameters* common_par){
+    common_par->shutdownMsg.data = common_par->shutdownMsg.data + ":" +
+                                    boost::replace_all_copy(ros::this_node::getName(), "/", "") +
+                                    ":" + std::to_string(ros::Time::now().toSec());
+    ROS_DEBUG("StartupMessage: %s", common_par->shutdownMsg.data.c_str());
+    publishers->find(common_par->ros_topics.interfacePub)->second->publish(common_par->shutdownMsg);
+}
+
+
+////////////////////////////////////////////////////
+/////////////// OTHER /////////////////////////////
+//////////////////////////////////////////////////

 geometry_msgs::TwistStamped jstr2Twist(nlohmann::json cval){
    static uint32_t seqId = 0;
@@ -61,8 +105,8 @@ geometry_msgs::PoseStamped twist2Pose(geometry_msgs::TwistStamped twt){
    newPoseStamp.pose.position.x = twt.twist.linear.x;
    newPoseStamp.pose.position.z = twt.twist.linear.z;
    // FIXME ripristinare il calcolo con le coordinate di posa dal tracker, ora gli assi sono fissi
-    //qt.setRPY(twPose.twist.angular.x, twPose.twist.angular.y, twPose.twist.angular.z);
-    qt.setRPY(0, M_PI/2, M_PI/2);
+    qt.setRPY(twt.twist.angular.x, twt.twist.angular.y, twt.twist.angular.z);
+    //qt.setRPY(0, M_PI/2, M_PI/2);
    newPoseStamp.pose.orientation.x = qt.x();
    newPoseStamp.pose.orientation.y = qt.y();
    newPoseStamp.pose.orientation.z = qt.z();
@@ -94,7 +138,7 @@ double deg2rad(double ang){
    return ang*M_PI/180;
 }

-bool checkJointJump(moveit_msgs::RobotTrajectoryPtr traj, double thresh){
+bool checkJointJump(moveit_msgs::RobotTrajectory *traj, double thresh){
    // identify max joint jump for each joint
    typedef struct{
        double first, second;
--- a/src/universal_robot
+++ b/src/universal_robot
--- a/src/universal_robots_ros_driver
+++ b/src/universal_robots_ros_driver
--- a/src/ur_modern_driver
+++ b/src/ur_modern_driver
Author	SHA1	Message	Date
Emanuele	02fce8a892	I nodi non sono segnati come Required Spegnimento grafecul attraverso interfaccia	2019-11-19 09:21:38 +01:00
Emanuele	ba5ed10349	Modifica angoli RPY per tenere utensile perpendicolare al terreno	2019-11-19 09:20:45 +01:00
Emanuele	dde6d1e083	Recorder continua a crashare a caso quando si apre file con plot	2019-11-15 17:01:45 +01:00
Emanuele	73ab44a67e	Aggiunti messaggi di debug ai nodi Crash di Recorder quando si cerca di plottare un percorso, il problema sta negli strumenti rViz, debug necessario	2019-11-15 16:45:29 +01:00
Emanuele	aef477e8e5	Crash alla partenza del nodo brodcaster	2019-11-15 14:46:16 +01:00
Emanuele	16eed0ad4e	Inizializzazione broadcaster con operazioni standard dei nodi	2019-11-15 12:22:50 +01:00
Emanuele	a3f75e5449	Ricompilazione con nuovo nodo TF broadcaster	2019-11-14 17:08:39 +01:00
Emanuele	c9108aaa54	Filet temporaei eliminati prima della ricompilazione	2019-11-14 16:10:59 +01:00
Emanuele	e3b88fe742	Aggiunto nodo Broadcaster continuo delle trasformazioni	2019-11-14 16:10:36 +01:00
Emanuele	d725017879	Plot del percorso con reference frame corrente e non world	2019-11-14 15:46:18 +01:00
Emanuele	270bf14a3e	Prima implementazione funzionante del cambio frame di riferimento	2019-11-13 15:55:23 +01:00
Emanuele	b98948ce17	aggiornato launch con rviz	2019-11-12 11:36:54 +01:00
Emanuele	b03c2e73bc	aggiunto parametro planning frame di default	2019-11-12 10:51:35 +01:00
Emanuele	b7f1857e5c	Inizia implementazione Set Frame	2019-11-12 10:51:15 +01:00
Emanuele	a0a36a310d	Nessun Cambiamento: solo apertura progetto	2019-11-05 16:48:32 +01:00
Emanuele	dd918d71af	Modificate le funzioni startup e heartbeat aggiunti timestamp, da aggiungere un modo per far emettere un messaggio di shutdown prima che il nodo venga killato.	2019-11-04 15:39:10 +01:00
Emanuele	e21d5a13e3	Primo tentativo di Override della gestione segnali shutdown	2019-11-04 14:29:47 +01:00
Emanuele	3f34c8fbc8	Aggiornato launch con nuovo parametro per shutdown	2019-11-04 11:14:16 +01:00
Emanuele	2e0361d108	Aggiunto messaggio shutdown del nodo	2019-11-04 11:12:43 +01:00
Emanuele	9b26f1979e	Aggiornati i messaggi di heartbeat da mandare a gui con timestamp	2019-10-30 17:28:47 +01:00
Emanuele	38ec9bfb3f	Aggiunta funzione di heartbeat sul topic interface	2019-10-29 14:17:18 +01:00
Emanuele	a9a82c5fe2	corerzioni nel caricamento dei parametri	2019-10-29 13:11:23 +01:00
Emanuele	8ff1c7e84a	Weaponizzata la logica che carica i parametri da ROSparam Raggruppati tutti i parametri comuni in una struttura uguale per tutti i nodi. Aggiungere altri parametri alla lista dei comuni	2019-10-29 11:47:18 +01:00
Emanuele	ad07d2f1ca	Aggiornalo launch con i nuovi topic	2019-10-29 11:06:33 +01:00
Emanuele	8778f69eb9	Aggiunti topi MQTT e ROS aggiunti topic interface per la comunicazione di stato tra i moduli e l'interfacia Qt (e viceversa)	2019-10-29 11:02:03 +01:00
Emanuele	db4a3fab14	Prove per il messaggio di startup non carica il messaggio dal server dei parametri, no si può pubblicare un tipo std::string in un topic ROS	2019-10-28 16:37:20 +01:00
Emanuele	5acd7d634d	Prima implementazione messaggio startup dei moduli	2019-10-28 15:39:40 +01:00
Emanuele	7e9dbe38e1	Detection dei salti di giunto funzionante	2019-10-28 14:27:45 +01:00
Emanuele	c56b427e47	Invio delle traiettorie in più segmenti Funzionante con il nuovo controller	2019-10-28 11:43:39 +01:00
				`@@ -0,0 +1 @@`
				`{"code":"{613d5288-5d28-465d-996d-db6750a6789c}","ds":0.5,"dt":0.5,"frame_base":"---","frame_work":{"name":"---","offset":{"r":0.0,"x":0.0,"y":0.0,"z":0.0}},"n_analog":0,"n_digital":0,"n_point":50,"name":"test99","type":"time/dist"}`