Prima implementazione messaggio startup dei moduli

src/roboglue_ros/launch/roboglue_ros_urdriver.launch

@@ -40,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" />
@@ -53,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/" />
@@ -69,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/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,7 @@ int main(int argc, char **argv) {
     /// node variables ///
     int loopRate, msgBufferSize, jogPubRate;
     std::string param_ns;
+    std_msgs::String startup_msg;
     ROS_topics ros_topics;
     /// internal state struct ////
     internalState is;
@@ -191,6 +192,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.data);
     nh.getParam(param_ns+"loop_rate", loopRate);
     nh.getParam(param_ns+"msg_buffer", msgBufferSize);
     nh.getParam(param_ns+"INstate", ros_topics.stateSub);
@@ -210,7 +212,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 +223,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 +261,18 @@ 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) {
+        if (startCycle){
+            startCycle = false;
+            publishers[command_pub.getTopic()]->publish(startup_msg);
+        }
         ROS_DEBUG_ONCE("Follower Node Looping");
-        //testFunction(&trj_pub);
         ros::spinOnce();
-        //if (is.isRealtime)
         loop_rate.sleep();
     }
+    //////////////////////////////////////////////////////
     return 0;
 }
 ////////////////////////////////////////

src/roboglue_ros/src/roboglue_recorder.cpp

@@ -33,7 +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 si Git
+20191029 - Spostata la documentazione suG GIT
 */
 ////////// ROS INCLUDES /////////////
 #include "ros/ros.h"
@@ -125,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;
@@ -181,22 +181,26 @@ 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());
-    nlohmann::json auxParsed = nlohmann::json::parse(msg->data);
+    try {
-    nlohmann::json digitalParsed = auxParsed["digital"];
+        nlohmann::json auxParsed = nlohmann::json::parse(msg->data);
-    nlohmann::json analogParsed = auxParsed["analog"];
+        nlohmann::json digitalParsed = auxParsed["digital"];
-    std::vector<std::pair<std::string,bool>> tempDigital;
+        nlohmann::json analogParsed = auxParsed["analog"];
-    std::vector<std::pair<std::string,uint16_t>> tempAnalog;
+        std::vector<std::pair<std::string,bool>> tempDigital;
-    if (is->isRecording || is->isRealtime){
+        std::vector<std::pair<std::string,uint16_t>> tempAnalog;
-        for (auto di : digitalParsed.items()){
+        if (is->isRecording || is->isRealtime){
-            tempDigital.push_back(std::pair<std::string,bool>(di.key(),di.value().get<bool>()));
+            for (auto di : digitalParsed.items()){
+                tempDigital.push_back(std::pair<std::string,bool>(di.key(),di.value().get<bool>()));
+            }
+            for (auto ai : analogParsed.items()){
+                tempAnalog.push_back(std::pair<std::string,uint16_t>(ai.key(),ai.value().get<uint16_t>()));
+            }
+            aux->digital.clear();
+            aux->analog.clear();
+            aux->digital.assign(tempDigital.begin(), tempDigital.end());
+            aux->analog.assign(tempAnalog.begin(), tempAnalog.end());
         }
-        for (auto ai : analogParsed.items()){
+    } catch (nlohmann::json::exception &e) {
-            tempAnalog.push_back(std::pair<std::string,uint16_t>(ai.key(),ai.value().get<uint16_t>()));
+         ROS_ERROR("Failed to parse STATE: [%s]", e.what());
-        }
-        aux->digital.clear();
-        aux->analog.clear();
-        aux->digital.assign(tempDigital.begin(), tempDigital.end());
-        aux->analog.assign(tempAnalog.begin(), tempAnalog.end());
     }
 }
 
@@ -224,6 +228,8 @@ int main(int argc, char **argv) {
     file_data.playVect = &playVector;
 
     /// player variables ///
+
+    std_msgs::String startup_msg;
     double dsCounter, dtCounter;
     std::vector<pointRecord> tempPlayVector;
     std::vector<geometry_msgs::PoseStamped> tempPlanningVector;
@@ -235,6 +241,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.data);
     nh.getParam(param_ns+"loop_rate", loopRate);
     nh.getParam(param_ns+"msg_buffer", msgBufferSize);
     nh.getParam(param_ns+"min_nonzero_move", nonzero_move);
@@ -311,7 +318,9 @@ int main(int argc, char **argv) {
             rviz_data.vtools->loadRemoteControl();
             rviz_data.vtools->deleteAllMarkers();
             rviz_data.vtools->trigger();
+            publishers[command_pub.getTopic()]->publish(startup_msg);
         }
+        ROS_DEBUG_ONCE("Recorder Node Looping");
         ros::spinOnce();
           /////////////////////////////////////////
          /////////// PLAY FILE ///////////////////

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,13 @@ int main(int argc, char **argv) {
     std::string param_ns;
     MQTT_topics mqtt_topics;
     ROS_topics ros_topics;
+    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.data);
     nh.getParam(param_ns+"loop_rate", loopRate);
     nh.getParam(param_ns+"msg_buffer", msgBufferSize);
     nh.getParam(param_ns+"INstate", ros_topics.stateSub);
@@ -146,13 +149,18 @@ 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;
+            publishers[command_pub.getTopic()]->publish(startup_msg);
+        }
         ROS_DEBUG_ONCE("Relay Node Looping");
         ros::spinOnce();
         loop_rate.sleep();
     }
-
+    ///////////////////////////////////////
     try {
         client.stop_consuming();
         client.disable_callbacks();