diff --git a/roboglue_ros_ws.workspace.user b/roboglue_ros_ws.workspace.user index 91ec781..745b9ad 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=392 + SHLVL=446 TERM=xterm _=/usr/bin/env diff --git a/src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch b/src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch index 04e8000..84e8b24 100644 --- a/src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch +++ b/src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch @@ -35,6 +35,7 @@ + @@ -42,7 +43,6 @@ - @@ -58,14 +58,12 @@ - - diff --git a/src/roboglue_ros/src/roboglue_follower.cpp b/src/roboglue_ros/src/roboglue_follower.cpp index 1fdabfc..4b55243 100644 --- a/src/roboglue_ros/src/roboglue_follower.cpp +++ b/src/roboglue_ros/src/roboglue_follower.cpp @@ -114,9 +114,15 @@ void stateCallback(const std_msgs::String::ConstPtr& msg, internalState* is, aux } void interfaceCallback(const std_msgs::String::ConstPtr& msg, internalState *is){ - ROS_DEBUG("Received Interface Message; [%s]", msg->data.c_str()); + ROS_WARN("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 timedPublishCallback(const ros::TimerEvent&, internalState* is, publisherMap* pubs, positionData* pos, robotData* rdata){ static geometry_msgs::TwistStamped lasttwPose; static trajectory_msgs::JointTrajectory lastjtPose; @@ -258,7 +264,8 @@ int main(int argc, char **argv) { /// create timed callbacks /// ros::Timer timedPublish = nh.createTimer(ros::Duration(1.0/static_cast(jogPubRate)), boost::bind(timedPublishCallback, _1, &is, &publishers, &pos_data, &robot_data)); - // now we can get inverse kinematics from kinematic state using the joint model + ros::Timer heartBeat = nh.createTimer(ros::Duration(5.0), + boost::bind(heartBeatCallback, _1, &publishers)); ///////////////////// MAIN LOOP ////////////////////// bool startCycle = true; @@ -266,7 +273,8 @@ int main(int argc, char **argv) { if (startCycle){ startCycle = false; common_par.startupMsg.data = common_par.startupMsg.data + ros::this_node::getName(); - publishers[command_pub.getTopic()]->publish(common_par.startupMsg); + ROS_DEBUG("StartupMessage: %s", common_par.startupMsg.data.c_str()); + publishers[interface_pub.getTopic()]->publish(common_par.startupMsg); ros::spinOnce(); } ROS_DEBUG_ONCE("Follower Node Looping"); diff --git a/src/roboglue_ros/src/roboglue_recorder.cpp b/src/roboglue_ros/src/roboglue_recorder.cpp index 02f3bfe..9d05dab 100644 --- a/src/roboglue_ros/src/roboglue_recorder.cpp +++ b/src/roboglue_ros/src/roboglue_recorder.cpp @@ -205,9 +205,15 @@ void stateCallback(const std_msgs::String::ConstPtr& msg, internalState*is, auxD } void interfaceCallback(const std_msgs::String::ConstPtr& msg, internalState *is){ - ROS_DEBUG("Received Interface Message; [%s]", msg->data.c_str()); + ROS_WARN("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); +}; + //////////////////////////////////////// //////////////// MAIN ////////////////// //////////////////////////////////////// @@ -296,6 +302,10 @@ int main(int argc, char **argv) { publishers[state_pub.getTopic()] = &state_pub; publishers[interface_pub.getTopic()] = &interface_pub; + /// create timed callbacks /// + ros::Timer heartBeat = nh.createTimer(ros::Duration(5.0), + boost::bind(heartBeatCallback, _1, &publishers)); + /// load the robot model for inverse kinematics and self collision checking /// /// requires parameter server and moveIT to be active moveit::planning_interface::MoveGroupInterface move_group(robot_data.move_group_name); @@ -318,7 +328,8 @@ int main(int argc, char **argv) { rviz_data.vtools->deleteAllMarkers(); rviz_data.vtools->trigger(); common_par.startupMsg.data = common_par.startupMsg.data + ros::this_node::getName(); - publishers[command_pub.getTopic()]->publish(common_par.startupMsg); + ROS_DEBUG("StartupMessage: %s", common_par.startupMsg.data.c_str()); + publishers[interface_pub.getTopic()]->publish(common_par.startupMsg); ros::spinOnce(); } ROS_DEBUG_ONCE("Recorder Node Looping"); diff --git a/src/roboglue_ros/src/roboglue_relay.cpp b/src/roboglue_ros/src/roboglue_relay.cpp index d9853f9..9ea91c9 100644 --- a/src/roboglue_ros/src/roboglue_relay.cpp +++ b/src/roboglue_ros/src/roboglue_relay.cpp @@ -38,23 +38,22 @@ class mqtt2rosClass: public virtual mqtt::callback { ROS_DEBUG("MessageDelivered, t:%d",tok->get_message_id()); } void message_arrived(mqtt::const_message_ptr msg) override{ - ROS_DEBUG("MqttMessage!"); std::string topic(msg->get_topic()); std::string payload(msg->get_payload_str()); std_msgs::String rosMsg; rosMsg.data = payload; if (!topic.compare(mqtt_data_.MQTTcommandSub)){ - ROS_DEBUG("Command! -> %s", ros_data_.commandSub.c_str()); + ROS_DEBUG("MQTT Message: Command! -> %s", ros_data_.commandSub.c_str()); pubs_[ros_data_.commandSub]->publish(rosMsg); } else if (!topic.compare(mqtt_data_.MQTTcoordSub)){ - ROS_DEBUG("Coordinate! -> %s", ros_data_.coordSub.c_str()); + ROS_DEBUG("MQTT Message: Coordinate! -> %s", ros_data_.coordSub.c_str()); pubs_[ros_data_.coordSub]->publish(rosMsg); } else if (!topic.compare(mqtt_data_.MQTTstateSub)){ - ROS_DEBUG("State! -> %s", ros_data_.stateSub.c_str()); + ROS_DEBUG("MQTT Message: State! -> %s", ros_data_.stateSub.c_str()); pubs_[ros_data_.stateSub]->publish(rosMsg); } else if (!topic.compare(mqtt_data_.MQTTinterfaceSub)) { - ROS_DEBUG("Interface! -> %s", ros_data_.interfaceSub.c_str()); + ROS_DEBUG("MQTT Message: Interface! -> %s", ros_data_.interfaceSub.c_str()); pubs_[ros_data_.interfaceSub]->publish(rosMsg); } else { ROS_ERROR("Topic NOT Implemented"); @@ -66,49 +65,42 @@ class mqtt2rosClass: public virtual mqtt::callback { /////////////// ROS CALLBACKS ////////// //////////////////////////////////////// void commandCallback(const std_msgs::String::ConstPtr msg, mqtt::async_client* cli_, MQTT_topics* mqtttopics){ - ROS_DEBUG("RosMessage: Command!"); + ROS_DEBUG("ROS Message: Command! -> %s", msg->data.c_str()); cli_->publish(mqtt::make_message(mqtttopics->MQTTcommandPub, msg->data.c_str())); } void poseCallback(const std_msgs::String::ConstPtr msg, mqtt::async_client* cli_, MQTT_topics* mqtttopics){ - ROS_DEBUG("RosMessage: Pose!"); + ROS_DEBUG("ROS Message: Pose! -> %s", msg->data.c_str()); cli_->publish(mqtt::make_message(mqtttopics->MQTTcoordPub, msg->data.c_str())); } void stateCallback(const std_msgs::String::ConstPtr msg, mqtt::async_client* cli_, MQTT_topics* mqtttopics){ - ROS_DEBUG("RosMessage: State!"); + ROS_DEBUG("ROS Message: State! -> %s", msg->data.c_str()); cli_->publish(mqtt::make_message(mqtttopics->MQTTstatePub, msg->data.c_str())); } void interfaceCallback(const std_msgs::String::ConstPtr msg, mqtt::async_client* cli_, MQTT_topics* mqtttopics){ - ROS_DEBUG("RosMessage: Interface!"); + 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){ + std_msgs::String hbMsg; + hbMsg.data = std::string("HB/"+ros::this_node::getName()); + p->at("/roboglue_com/ros2com/interface")->publish(hbMsg); +}; + //////////////////////////////////////// //////////////// MAIN ////////////////// //////////////////////////////////////// int main(int argc, char **argv) { /// node variables /// - int loopRate, msgBufferSize; - std::string param_ns; + + COMMON_parameters common_par; 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); - nh.getParam(param_ns+"INcommands", ros_topics.commandSub); - nh.getParam(param_ns+"INcoordinates", ros_topics.coordSub); - nh.getParam(param_ns+"INinterface", ros_topics.interfaceSub); - nh.getParam(param_ns+"OUTstate", ros_topics.statePub); - nh.getParam(param_ns+"OUTcommands", ros_topics.commandPub); - nh.getParam(param_ns+"OUTcoordinates", ros_topics.coordPub); - nh.getParam(param_ns+"OUTinterface", ros_topics.interfacePub); + nh.getParam("/roboglue_ros_relay/parameter_ns", common_par.param_ns); + loadCommonParameters(&nh, &common_par); nh.getParam("/roboglue_ros_relay/mqtt_INstate", mqtt_topics.MQTTstateSub); nh.getParam("/roboglue_ros_relay/mqtt_INcommands", mqtt_topics.MQTTcommandSub); nh.getParam("/roboglue_ros_relay/mqtt_INcoordinates", mqtt_topics.MQTTcoordSub); @@ -121,7 +113,7 @@ int main(int argc, char **argv) { nh.getParam("/roboglue_ros_relay/mqtt_qos", mqtt_topics.mqttQoS); /// set spinner rate /// - ros::Rate loop_rate(loopRate); + ros::Rate loop_rate(common_par.loopRate); /// set console log level /// ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug); ROS_INFO("Relay Node Started"); @@ -130,28 +122,31 @@ int main(int argc, char **argv) { /// advertise publish topics /// publisherMap publishers; - ros::Publisher command_pub = nh.advertise(ros_topics.commandSub,static_cast(msgBufferSize)); - ros::Publisher pose_pub = nh.advertise(ros_topics.coordSub, static_cast(msgBufferSize)); - ros::Publisher state_pub = nh.advertise(ros_topics.stateSub,static_cast(msgBufferSize)); - ros::Publisher interface_pub = nh.advertise(ros_topics.interfaceSub,static_cast(msgBufferSize)); + ros::Publisher command_pub = nh.advertise(common_par.ros_topics.commandSub,static_cast(common_par.msgBufferSize)); + 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)); /// 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; /// create mqtt callback /// - mqtt2rosClass mqtt2ros(client, publishers, mqtt_topics, ros_topics); + mqtt2rosClass mqtt2ros(client, publishers, mqtt_topics, common_par.ros_topics); /// subscribe to incoming topics /// - ros::Subscriber command_sub = nh.subscribe(ros_topics.commandPub,static_cast(msgBufferSize), + ros::Subscriber command_sub = nh.subscribe(common_par.ros_topics.commandPub,static_cast(common_par.msgBufferSize), boost::bind(commandCallback, _1, &client, &mqtt_topics)); - ros::Subscriber pose_sub = nh.subscribe(ros_topics.coordPub,static_cast(msgBufferSize), + ros::Subscriber pose_sub = nh.subscribe(common_par.ros_topics.coordPub,static_cast(common_par.msgBufferSize), boost::bind(poseCallback, _1, &client, &mqtt_topics)); - ros::Subscriber state_sub = nh.subscribe(ros_topics.statePub,static_cast(msgBufferSize), + ros::Subscriber state_sub = nh.subscribe(common_par.ros_topics.statePub,static_cast(common_par.msgBufferSize), boost::bind(stateCallback, _1, &client, &mqtt_topics)); - ros::Subscriber interface_sub = nh.subscribe(ros_topics.interfacePub,static_cast(msgBufferSize), + ros::Subscriber interface_sub = nh.subscribe(common_par.ros_topics.interfacePub,static_cast(common_par.msgBufferSize), boost::bind(interfaceCallback, _1, &client, &mqtt_topics)); + /// create timed callbacks /// + ros::Timer heartBeat = nh.createTimer(ros::Duration(5.0), + boost::bind(heartBeatCallback, _1, &publishers)); /// CONNECT MQTT CLIENT /// ROS_INFO("Connecting to MQTT..."); try { @@ -171,8 +166,9 @@ int main(int argc, char **argv) { while (ros::ok()) { if (startCycle){ startCycle = false; - startup_msg.data = startup_msg_tmp.c_str(); - publishers[command_pub.getTopic()]->publish(startup_msg); + 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); ros::spinOnce(); } ROS_DEBUG_ONCE("Relay Node Looping");