From 9b26f1979e76cab8a0d5d6dda827c99043c04f38 Mon Sep 17 00:00:00 2001 From: Emanuele Date: Wed, 30 Oct 2019 17:28:47 +0100 Subject: [PATCH] Aggiornati i messaggi di heartbeat da mandare a gui con timestamp --- roboglue_ros_ws.workspace.user | 8 ++++---- src/roboglue_ros/src/roboglue_follower.cpp | 16 ++++++++++------ src/roboglue_ros/src/roboglue_recorder.cpp | 16 ++++++++++------ src/roboglue_ros/src/roboglue_relay.cpp | 18 ++++++++++++------ src/ur_modern_driver | 2 +- 5 files changed, 37 insertions(+), 23 deletions(-) diff --git a/roboglue_ros_ws.workspace.user b/roboglue_ros_ws.workspace.user index 745b9ad..4ab9b5e 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=446 + SHLVL=528 TERM=xterm _=/usr/bin/env @@ -559,8 +559,8 @@ true roboglue_ros /home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros - roboglue_ros_recorder - /home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/devel/lib/roboglue_ros/roboglue_ros_recorder + roboglue_ros_relay + /home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/devel/lib/roboglue_ros/roboglue_ros_relay diff --git a/src/roboglue_ros/src/roboglue_follower.cpp b/src/roboglue_ros/src/roboglue_follower.cpp index 4b55243..e98b4c4 100644 --- a/src/roboglue_ros/src/roboglue_follower.cpp +++ b/src/roboglue_ros/src/roboglue_follower.cpp @@ -114,14 +114,18 @@ void stateCallback(const std_msgs::String::ConstPtr& msg, internalState* is, aux } void interfaceCallback(const std_msgs::String::ConstPtr& msg, internalState *is){ - ROS_WARN("Received Interface Message; [%s]", msg->data.c_str()); + ROS_DEBUG("Received Interface Message: [%s]", msg->data.c_str()); } -void heartBeatCallback(const ros::TimerEvent&, publisherMap* p){ - std_msgs::String hbMsg; - hbMsg.data = std::string("HB/"+ros::this_node::getName()); - p->at("/roboglue_com/ros2com/interface")->publish(hbMsg); -}; +void heartBeatCallback(const ros::TimerEvent& t, publisherMap* p){ + try { + std_msgs::String hbMsg; + hbMsg.data = std::string("HB/"+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; diff --git a/src/roboglue_ros/src/roboglue_recorder.cpp b/src/roboglue_ros/src/roboglue_recorder.cpp index 9d05dab..60e4288 100644 --- a/src/roboglue_ros/src/roboglue_recorder.cpp +++ b/src/roboglue_ros/src/roboglue_recorder.cpp @@ -205,14 +205,18 @@ void stateCallback(const std_msgs::String::ConstPtr& msg, internalState*is, auxD } void interfaceCallback(const std_msgs::String::ConstPtr& msg, internalState *is){ - ROS_WARN("Received Interface Message; [%s]", msg->data.c_str()); + ROS_DEBUG("Received Interface Message: [%s]", msg->data.c_str()); } -void heartBeatCallback(const ros::TimerEvent&, publisherMap* p){ - std_msgs::String hbMsg; - hbMsg.data = std::string("HB/"+ros::this_node::getName()); - p->at("/roboglue_com/ros2com/interface")->publish(hbMsg); -}; +void heartBeatCallback(const ros::TimerEvent& t, publisherMap* p){ + try { + std_msgs::String hbMsg; + hbMsg.data = std::string("HB/"+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 ////////////////// diff --git a/src/roboglue_ros/src/roboglue_relay.cpp b/src/roboglue_ros/src/roboglue_relay.cpp index 9ea91c9..2cd1198 100644 --- a/src/roboglue_ros/src/roboglue_relay.cpp +++ b/src/roboglue_ros/src/roboglue_relay.cpp @@ -80,12 +80,15 @@ void interfaceCallback(const std_msgs::String::ConstPtr msg, mqtt::async_client* 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&, publisherMap* p){ +void heartBeatCallback(const ros::TimerEvent& t, publisherMap* p){ std_msgs::String hbMsg; - hbMsg.data = std::string("HB/"+ros::this_node::getName()); - p->at("/roboglue_com/ros2com/interface")->publish(hbMsg); -}; + try { + hbMsg.data = std::string("HB/"+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 ////////////////// @@ -115,7 +118,7 @@ int main(int argc, char **argv) { /// set spinner rate /// ros::Rate loop_rate(common_par.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("Relay Node Started"); /// initialize MQTT client /// mqtt::async_client client(mqtt_topics.mqttHost,""); @@ -126,11 +129,14 @@ int main(int argc, char **argv) { ros::Publisher pose_pub = nh.advertise(common_par.ros_topics.coordSub, static_cast(common_par.msgBufferSize)); ros::Publisher state_pub = nh.advertise(common_par.ros_topics.stateSub,static_cast(common_par.msgBufferSize)); ros::Publisher interface_pub = nh.advertise(common_par.ros_topics.interfaceSub,static_cast(common_par.msgBufferSize)); + ros::Publisher interface_pub2 = nh.advertise(common_par.ros_topics.interfacePub,static_cast(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, common_par.ros_topics); diff --git a/src/ur_modern_driver b/src/ur_modern_driver index 02b63f9..bf57198 160000 --- a/src/ur_modern_driver +++ b/src/ur_modern_driver @@ -1 +1 @@ -Subproject commit 02b63f9b75806423743d23e53be423a73e51ad1c +Subproject commit bf57198f3a0e5ac723d5c1499be88f3d6c0b36cb