diff --git a/roboglue_ros_ws.workspace.user b/roboglue_ros_ws.workspace.user index 4ab9b5e..d0c0972 100644 --- a/roboglue_ros_ws.workspace.user +++ b/roboglue_ros_ws.workspace.user @@ -1,6 +1,6 @@ - + EnvironmentId @@ -124,7 +124,7 @@ ROS_PACKAGE_PATH=/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src:/opt/ros/kinetic/share ROS_ROOT=/opt/ros/kinetic/share/ros ROS_VERSION=1 - SHLVL=528 + SHLVL=608 TERM=xterm _=/usr/bin/env diff --git a/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h b/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h index 1726e82..cf6e7a0 100644 --- a/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h +++ b/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h @@ -52,7 +52,7 @@ typedef struct { ROS_topics ros_topics; int loopRate, msgBufferSize; std::string param_ns; - std_msgs::String startupMsg; + std_msgs::String startupMsg, shutdownMsg; } COMMON_parameters; ///////// AUX DATA STORAGE /////////////// @@ -109,6 +109,22 @@ typedef struct { 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* recordVect; + std::vector* 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; @@ -116,6 +132,9 @@ typedef struct { ///////// 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); diff --git a/src/roboglue_ros/src/roboglue_follower.cpp b/src/roboglue_ros/src/roboglue_follower.cpp index e98b4c4..0458cf8 100644 --- a/src/roboglue_ros/src/roboglue_follower.cpp +++ b/src/roboglue_ros/src/roboglue_follower.cpp @@ -276,9 +276,7 @@ int main(int argc, char **argv) { while (ros::ok() && is.isRunning) { if (startCycle){ startCycle = false; - common_par.startupMsg.data = common_par.startupMsg.data + ros::this_node::getName(); - ROS_DEBUG("StartupMessage: %s", common_par.startupMsg.data.c_str()); - publishers[interface_pub.getTopic()]->publish(common_par.startupMsg); + declareStartup(&publishers, &common_par); ros::spinOnce(); } ROS_DEBUG_ONCE("Follower Node Looping"); @@ -286,6 +284,7 @@ int main(int argc, char **argv) { loop_rate.sleep(); } ////////////////////////////////////////////////////// + declareShutdown(&publishers, &common_par); return 0; } //////////////////////////////////////// diff --git a/src/roboglue_ros/src/roboglue_recorder.cpp b/src/roboglue_ros/src/roboglue_recorder.cpp index 60e4288..28c1f60 100644 --- a/src/roboglue_ros/src/roboglue_recorder.cpp +++ b/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 suG GIT +20191029 - Spostata la documentazione su GIT */ ////////// ROS INCLUDES ///////////// #include "ros/ros.h" @@ -43,22 +43,9 @@ #include #include -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* recordVect; - std::vector* 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); @@ -331,9 +318,7 @@ int main(int argc, char **argv) { rviz_data.vtools->loadRemoteControl(); rviz_data.vtools->deleteAllMarkers(); rviz_data.vtools->trigger(); - common_par.startupMsg.data = common_par.startupMsg.data + ros::this_node::getName(); - ROS_DEBUG("StartupMessage: %s", common_par.startupMsg.data.c_str()); - publishers[interface_pub.getTopic()]->publish(common_par.startupMsg); + declareStartup(&publishers, &common_par); ros::spinOnce(); } ROS_DEBUG_ONCE("Recorder Node Looping"); @@ -445,6 +430,7 @@ int main(int argc, char **argv) { } loop_rate.sleep(); } + declareShutdown(&publishers, &common_par); return 0; } //////////////////////////////////////// diff --git a/src/roboglue_ros/src/roboglue_relay.cpp b/src/roboglue_ros/src/roboglue_relay.cpp index 2cd1198..161284e 100644 --- a/src/roboglue_ros/src/roboglue_relay.cpp +++ b/src/roboglue_ros/src/roboglue_relay.cpp @@ -172,9 +172,7 @@ int main(int argc, char **argv) { while (ros::ok()) { if (startCycle){ startCycle = false; - common_par.startupMsg.data = common_par.startupMsg.data + ros::this_node::getName(); - ROS_DEBUG("StartupMessage: %s", common_par.startupMsg.data.c_str()); - publishers[interface_pub.getTopic()]->publish(common_par.startupMsg); + declareStartup(&publishers, &common_par); ros::spinOnce(); } ROS_DEBUG_ONCE("Relay Node Looping"); @@ -189,6 +187,6 @@ int main(int argc, char **argv) { } catch (mqtt::exception &e) { ROS_DEBUG("Error Disconnecting MQTT: [%s]", e.what()); } - + declareShutdown(&publishers, &common_par); return 0; } diff --git a/src/roboglue_ros/src/roboglue_utils.cpp b/src/roboglue_ros/src/roboglue_utils.cpp index b406195..d867319 100644 --- a/src/roboglue_ros/src/roboglue_utils.cpp +++ b/src/roboglue_ros/src/roboglue_utils.cpp @@ -17,6 +17,19 @@ bool loadCommonParameters(ros::NodeHandle* nh, COMMON_parameters *p) { 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); +} + +void declareStartup(publisherMap* publishers, COMMON_parameters* common_par){ + common_par->startupMsg.data = common_par->startupMsg.data + ros::this_node::getName(); + 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 + ros::this_node::getName(); + ROS_DEBUG("StartupMessage: %s", common_par->shutdownMsg.data.c_str()); + publishers->find(common_par->ros_topics.interfacePub)->second->publish(common_par->shutdownMsg); } geometry_msgs::TwistStamped jstr2Twist(nlohmann::json cval){