diff --git a/src/roboglue_ros/launch/roboglue_ros_urdriver.launch b/src/roboglue_ros/launch/roboglue_ros_urdriver.launch index ddedb96..8cb9dd0 100644 --- a/src/roboglue_ros/launch/roboglue_ros_urdriver.launch +++ b/src/roboglue_ros/launch/roboglue_ros_urdriver.launch @@ -40,6 +40,7 @@ + @@ -53,12 +54,14 @@ + + @@ -69,7 +72,4 @@ - - - diff --git a/src/roboglue_ros/src/roboglue_follower.cpp b/src/roboglue_ros/src/roboglue_follower.cpp index 5685bb3..f25460c 100644 --- a/src/roboglue_ros/src/roboglue_follower.cpp +++ b/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){ - ROS_ERROR("Failed to parse command!"); + } catch (nlohmann::json::exception &e){ + 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(msgBufferSize)); ros::Publisher trj_pub = nh.advertise(ros_topics.traj_jog, static_cast(msgBufferSize)); + ros::Publisher command_pub = nh.advertise(ros_topics.commandPub, + static_cast(msgBufferSize)); + ros::Publisher coord_pub = nh.advertise(ros_topics.coordPub, + static_cast(msgBufferSize)); + ros::Publisher state_pub = nh.advertise(ros_topics.statePub, + static_cast(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(ros_topics.commandSub, static_cast(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; } //////////////////////////////////////// diff --git a/src/roboglue_ros/src/roboglue_recorder.cpp b/src/roboglue_ros/src/roboglue_recorder.cpp index 60b71b6..881cea8 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 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); - nlohmann::json digitalParsed = auxParsed["digital"]; - nlohmann::json analogParsed = auxParsed["analog"]; - std::vector> tempDigital; - std::vector> tempAnalog; - if (is->isRecording || is->isRealtime){ - for (auto di : digitalParsed.items()){ - tempDigital.push_back(std::pair(di.key(),di.value().get())); + try { + nlohmann::json auxParsed = nlohmann::json::parse(msg->data); + nlohmann::json digitalParsed = auxParsed["digital"]; + nlohmann::json analogParsed = auxParsed["analog"]; + std::vector> tempDigital; + std::vector> tempAnalog; + if (is->isRecording || is->isRealtime){ + for (auto di : digitalParsed.items()){ + tempDigital.push_back(std::pair(di.key(),di.value().get())); + } + for (auto ai : analogParsed.items()){ + tempAnalog.push_back(std::pair(ai.key(),ai.value().get())); + } + 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()){ - tempAnalog.push_back(std::pair(ai.key(),ai.value().get())); - } - aux->digital.clear(); - aux->analog.clear(); - 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()); } } @@ -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 tempPlayVector; std::vector 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 /////////////////// diff --git a/src/roboglue_ros/src/roboglue_relay.cpp b/src/roboglue_ros/src/roboglue_relay.cpp index a98f734..20bb639 100644 --- 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 ///////////// @@ -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();