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

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.1, 2019-10-28T14:03:33. -->
 <qtcreator>
  <data>
   <variable>EnvironmentId</variable>
@@ -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=392</value>
      <value type="QString">TERM=xterm</value>
      <value type="QString">_=/usr/bin/env</value>
     </valuelist>
@@ -550,6 +550,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_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>
+     <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 +575,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 +584,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>

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

src/roboglue_ros/include/roboglue_ros/roboglue_utils.h

@@ -98,6 +98,7 @@ 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;
 
 typedef struct {
@@ -114,4 +115,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);

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>
 

src/roboglue_ros/launch/roboglue_ros_urdriver.launch

@@ -33,12 +33,14 @@
         <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>
 
     <!-- Message Relay to/from MQTT -->
     <node name="roboglue_ros_relay" pkg="roboglue_ros" type="roboglue_ros_relay" output="screen" required="true">
         <param name="parameter_ns" type="string" value="/roboglue_ros_common/" />
+        <param name="startup_msg" type="string" value='{"STARTUP":"RELAY"}'/>
         <param name="mqtt_host" type="string" value="tcp://localhost:1883" />
         <param name="mqtt_qos" type="int" value="0" />
         <param name="mqtt_INstate" type="string" value="roboglue_com/com2ros/state" />
@@ -52,12 +54,14 @@
     <!-- 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" />
+        <param name="startup_msg" type="string" value='{"STARTUP":"FOLLOWER"}'/>
         <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">
         <param name="parameter_ns" type="string" value="/roboglue_ros_common/" />
+        <param name="startup_msg" type="string" value='{"STARTUP":"RECORDER"}'/>
         <param name="point_group_mode" type="string" value="dist" />
         <param name="planning_mode" type="string" value="path" />
         <param name="meta_dir" type="string" value="/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros/config/meta/" />
@@ -68,7 +72,4 @@
         <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" /> -->
-
 </launch>

src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch

@@ -30,6 +30,7 @@
         <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" />
         <rosparam command="load" file="$(find roboglue_ros)/config/jointDefaults.yaml" />
         <!-- ROS node loop parameters -->
         <param name="loop_rate" type="int" value="100" />
@@ -39,6 +40,7 @@
     <!-- Message Relay to/from MQTT -->
     <node name="roboglue_ros_relay" pkg="roboglue_ros" type="roboglue_ros_relay" output="screen" required="true">
         <param name="parameter_ns" type="string" value="/roboglue_ros_common/" />
+        <param name="startup_msg" type="string" value='{"STARTUP":"RELAY"}'/>
         <param name="mqtt_host" type="string" value="tcp://localhost:1883" />
         <param name="mqtt_qos" type="int" value="0" />
         <param name="mqtt_INstate" type="string" value="roboglue_com/com2ros/state" />
@@ -52,12 +54,14 @@
     <!-- 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" />
+        <param name="startup_msg" type="string" value='{"STARTUP":"FOLLOWER"}'/>
         <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">
         <param name="parameter_ns" type="string" value="/roboglue_ros_common/" />
+        <param name="startup_msg" type="string" value='{"STARTUP":"RECORDER"}'/>
         <param name="point_group_mode" type="string" value="dist" />
         <param name="planning_mode" type="string" value="path" />
         <param name="meta_dir" type="string" value="/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros/config/meta/" />

src/roboglue_ros/src/roboglue_follower.cpp

@@ -9,7 +9,7 @@
 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"
@@ -64,8 +64,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){
+    } catch (nlohmann::json::exception &e){
-        ROS_ERROR("Failed to parse command!");
+        ROS_ERROR("Failed to parse COMMAND: [%s]", e.what());
     }
 }
 
@@ -178,6 +178,8 @@ int main(int argc, char **argv) {
     /// node variables ///
     int loopRate, msgBufferSize, jogPubRate;
     std::string param_ns;
+    std::string startup_msg_tmp;
+    std_msgs::String startup_msg;
     ROS_topics ros_topics;
     /// internal state struct ////
     internalState is;
@@ -191,6 +193,7 @@ int main(int argc, char **argv) {
     ros::NodeHandle nh;
     ///read parameters from server ///
     nh.getParam("/roboglue_ros_relay/parameter_ns", param_ns);
+    nh.getParam(param_ns+"startup_msg", startup_msg_tmp);
     nh.getParam(param_ns+"loop_rate", loopRate);
     nh.getParam(param_ns+"msg_buffer", msgBufferSize);
     nh.getParam(param_ns+"INstate", ros_topics.stateSub);
@@ -210,7 +213,7 @@ int main(int argc, char **argv) {
     /// 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::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
     ROS_INFO("Follower Node Started");
 
     /// advertise publish topics ///
@@ -221,10 +224,22 @@ int main(int argc, char **argv) {
                                                                              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 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));
+
     /// 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 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));
@@ -247,13 +262,20 @@ int main(int argc, char **argv) {
                                              boost::bind(timedPublishCallback, _1, &is, &publishers, &pos_data, &robot_data));
     // now we can get inverse kinematics from kinematic state using the joint model
 
+    ///////////////////// MAIN LOOP //////////////////////
+    bool startCycle = true;
     while (ros::ok() && is.isRunning) {
-        ROS_DEBUG_ONCE("Follower Node Looping");
+        if (startCycle){
-        //testFunction(&trj_pub);
+            startCycle = false;
+            startup_msg.data = startup_msg_tmp.c_str();
+            publishers[command_pub.getTopic()]->publish(startup_msg);
+            ros::spinOnce();
+        }
+        ROS_DEBUG_ONCE("Follower Node Looping");
         ros::spinOnce();
-        //if (is.isRealtime)
         loop_rate.sleep();
     }
+    //////////////////////////////////////////////////////
     return 0;
 }
 ////////////////////////////////////////

src/roboglue_ros/src/roboglue_recorder.cpp

@@ -33,6 +33,7 @@
             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 suG GIT
 */
 ////////// ROS INCLUDES /////////////
 #include "ros/ros.h"
@@ -124,7 +125,7 @@ void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, p
             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 +181,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 +199,9 @@ 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());
+    }
 }
 
 ////////////////////////////////////////
@@ -223,6 +228,9 @@ int main(int argc, char **argv) {
     file_data.playVect = &playVector;
 
     /// player variables ///
+
+    std_msgs::String startup_msg;
+    std::string startup_msg_tmp;
     double dsCounter, dtCounter;
     std::vector<pointRecord> tempPlayVector;
     std::vector<geometry_msgs::PoseStamped> tempPlanningVector;
@@ -234,6 +242,7 @@ int main(int argc, char **argv) {
     /// read parameters from server ///
     //load common parameters
     nh.getParam("/roboglue_ros_recorder/parameter_ns", param_ns);
+    nh.getParam(param_ns+"startup_msg", startup_msg_tmp);
     nh.getParam(param_ns+"loop_rate", loopRate);
     nh.getParam(param_ns+"msg_buffer", msgBufferSize);
     nh.getParam(param_ns+"min_nonzero_move", nonzero_move);
@@ -260,6 +269,7 @@ int main(int argc, char **argv) {
     // 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);
+    nh.getParam(param_ns+"joint_jump_tresh", robot_data.joint_jump_tresh);
 
     /// set console log level ///
     ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Warn);
@@ -309,7 +319,11 @@ int main(int argc, char **argv) {
             rviz_data.vtools->loadRemoteControl();
             rviz_data.vtools->deleteAllMarkers();
             rviz_data.vtools->trigger();
+            startup_msg.data = startup_msg_tmp.c_str();
+            publishers[command_pub.getTopic()]->publish(startup_msg);
+            ros::spinOnce();
         }
+        ROS_DEBUG_ONCE("Recorder Node Looping");
         ros::spinOnce();
           /////////////////////////////////////////
          /////////// PLAY FILE ///////////////////
@@ -347,6 +361,9 @@ int main(int argc, char **argv) {
                     tempPlanningVector.back().header.frame_id="/world";
                 }
                 //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 +379,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.
+                    // check for joint jump and if not detected execute motion
-//                    if (checkJointJump(boost::shared_ptr<moveit_msgs::RobotTrajectory>(&traj), 90.0)){
+                    if (checkJointJump(&traj, robot_data.joint_jump_tresh)){
-//                        ROS_WARN("Joint Jump Detected!!!");
+                        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!");

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 /////////////
@@ -83,11 +84,14 @@ int main(int argc, char **argv) {
     std::string param_ns;
     MQTT_topics mqtt_topics;
     ROS_topics ros_topics;
+    std::string startup_msg_tmp;
+    std_msgs::String startup_msg;
     /// 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+"startup_msg", startup_msg_tmp);
     nh.getParam(param_ns+"loop_rate", loopRate);
     nh.getParam(param_ns+"msg_buffer", msgBufferSize);
     nh.getParam(param_ns+"INstate", ros_topics.stateSub);
@@ -146,13 +150,20 @@ int main(int argc, char **argv) {
         ROS_ERROR("Failed to Connect to MQTT: [%s]", e.what());
         ros::shutdown();
     }
-
+    ////////////// MAIN LOOP /////////////
+    bool startCycle = true;
     while (ros::ok()) {
+        if (startCycle){
+            startCycle = false;
+            startup_msg.data = startup_msg_tmp.c_str();
+            publishers[command_pub.getTopic()]->publish(startup_msg);
+            ros::spinOnce();
+        }
         ROS_DEBUG_ONCE("Relay Node Looping");
         ros::spinOnce();
         loop_rate.sleep();
     }
-
+    ///////////////////////////////////////
     try {
         client.stop_consuming();
         client.disable_callbacks();

src/roboglue_ros/src/roboglue_utils.cpp

@@ -94,7 +94,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;