Obbart/RoboGlue_ROS
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Aggiunta funzione di heartbeat sul topic interface

Browse Source
This commit is contained in:
Emanuele
2019-10-29 14:17:18 +01:00
parent a9a82c5fe2
commit 38ec9bfb3f
5 changed files with 61 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
roboglue_ros_ws.workspace.user
View File

@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject> <!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 4.9.1, 2019-10-28T14:03:33. --> <!-- Written by QtCreator 4.9.1, 2019-10-29T13:19:39. -->
<qtcreator> <qtcreator>
<data> <data>
<variable>EnvironmentId</variable> <variable>EnvironmentId</variable>
@@ -124,7 +124,7 @@
<value type="QString">ROS_PACKAGE_PATH=/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src:/opt/ros/kinetic/share</value> <value type="QString">ROS_PACKAGE_PATH=/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src:/opt/ros/kinetic/share</value>
<value type="QString">ROS_ROOT=/opt/ros/kinetic/share/ros</value> <value type="QString">ROS_ROOT=/opt/ros/kinetic/share/ros</value>
<value type="QString">ROS_VERSION=1</value> <value type="QString">ROS_VERSION=1</value>
<value type="QString">SHLVL=392</value> <value type="QString">SHLVL=446</value>
<value type="QString">TERM=xterm</value> <value type="QString">TERM=xterm</value>
<value type="QString">_=/usr/bin/env</value> <value type="QString">_=/usr/bin/env</value>
</valuelist> </valuelist>

4
src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch
View File

@@ -35,6 +35,7 @@
<param name="joint_jump_tresh" type="double" value="45.0" /> <param name="joint_jump_tresh" type="double" value="45.0" />
<rosparam command="load" file="$(find roboglue_ros)/config/jointDefaults.yaml" /> <rosparam command="load" file="$(find roboglue_ros)/config/jointDefaults.yaml" />
<!-- ROS node loop parameters --> <!-- ROS node loop parameters -->
<param name="startup_msg" type="string" value="STARTUP/"/>
<param name="loop_rate" type="int" value="100" /> <param name="loop_rate" type="int" value="100" />
<param name="msg_buffer" type="int" value="100" /> <param name="msg_buffer" type="int" value="100" />
</group> </group>
@@ -42,7 +43,6 @@
<!-- Message Relay to/from MQTT --> <!-- Message Relay to/from MQTT -->
<node name="roboglue_ros_relay" pkg="roboglue_ros" type="roboglue_ros_relay" output="screen" required="true"> <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="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_host" type="string" value="tcp://localhost:1883" />
<param name="mqtt_qos" type="int" value="0" /> <param name="mqtt_qos" type="int" value="0" />
<param name="mqtt_INstate" type="string" value="roboglue_com/com2ros/state" /> <param name="mqtt_INstate" type="string" value="roboglue_com/com2ros/state" />
@@ -58,14 +58,12 @@
<!-- Real Time Tracking --> <!-- Real Time Tracking -->
<node name="roboglue_ros_follower" pkg="roboglue_ros" type="roboglue_ros_follower" output="screen" required="true"> <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="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" /> <param name="jog_pub_rate" type="int" value="5" />
</node> </node>
<!-- Tracking data Recorder/Player --> <!-- Tracking data Recorder/Player -->
<node name="roboglue_ros_recorder" pkg="roboglue_ros" type="roboglue_ros_recorder" output="screen" required="true"> <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="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="point_group_mode" type="string" value="dist" />
<param name="planning_mode" type="string" value="path" /> <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/" /> <param name="meta_dir" type="string" value="/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros/config/meta/" />

14
src/roboglue_ros/src/roboglue_follower.cpp
View File

@@ -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){ 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){ void timedPublishCallback(const ros::TimerEvent&, internalState* is, publisherMap* pubs, positionData* pos, robotData* rdata){
static geometry_msgs::TwistStamped lasttwPose; static geometry_msgs::TwistStamped lasttwPose;
static trajectory_msgs::JointTrajectory lastjtPose; static trajectory_msgs::JointTrajectory lastjtPose;
@@ -258,7 +264,8 @@ int main(int argc, char **argv) {
/// create timed callbacks /// /// create timed callbacks ///
ros::Timer timedPublish = nh.createTimer(ros::Duration(1.0/static_cast<double>(jogPubRate)), ros::Timer timedPublish = nh.createTimer(ros::Duration(1.0/static_cast<double>(jogPubRate)),
boost::bind(timedPublishCallback, _1, &is, &publishers, &pos_data, &robot_data)); 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 ////////////////////// ///////////////////// MAIN LOOP //////////////////////
bool startCycle = true; bool startCycle = true;
@@ -266,7 +273,8 @@ int main(int argc, char **argv) {
if (startCycle){ if (startCycle){
startCycle = false; startCycle = false;
common_par.startupMsg.data = common_par.startupMsg.data + ros::this_node::getName(); 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::spinOnce();
} }
ROS_DEBUG_ONCE("Follower Node Looping"); ROS_DEBUG_ONCE("Follower Node Looping");

15
src/roboglue_ros/src/roboglue_recorder.cpp
View File

@@ -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){ 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 ////////////////// //////////////// MAIN //////////////////
//////////////////////////////////////// ////////////////////////////////////////
@@ -296,6 +302,10 @@ int main(int argc, char **argv) {
publishers[state_pub.getTopic()] = &state_pub; publishers[state_pub.getTopic()] = &state_pub;
publishers[interface_pub.getTopic()] = &interface_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 /// /// load the robot model for inverse kinematics and self collision checking ///
/// requires parameter server and moveIT to be active /// requires parameter server and moveIT to be active
moveit::planning_interface::MoveGroupInterface move_group(robot_data.move_group_name); 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->deleteAllMarkers();
rviz_data.vtools->trigger(); rviz_data.vtools->trigger();
common_par.startupMsg.data = common_par.startupMsg.data + ros::this_node::getName(); 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::spinOnce();
} }
ROS_DEBUG_ONCE("Recorder Node Looping"); ROS_DEBUG_ONCE("Recorder Node Looping");

72
src/roboglue_ros/src/roboglue_relay.cpp
View File

@@ -38,23 +38,22 @@ class mqtt2rosClass: public virtual mqtt::callback {
ROS_DEBUG("MessageDelivered, t:%d",tok->get_message_id()); ROS_DEBUG("MessageDelivered, t:%d",tok->get_message_id());
} }
void message_arrived(mqtt::const_message_ptr msg) override{ void message_arrived(mqtt::const_message_ptr msg) override{
ROS_DEBUG("MqttMessage!");
std::string topic(msg->get_topic()); std::string topic(msg->get_topic());
std::string payload(msg->get_payload_str()); std::string payload(msg->get_payload_str());
std_msgs::String rosMsg; std_msgs::String rosMsg;
rosMsg.data = payload; rosMsg.data = payload;
if (!topic.compare(mqtt_data_.MQTTcommandSub)){ 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); pubs_[ros_data_.commandSub]->publish(rosMsg);
} else if (!topic.compare(mqtt_data_.MQTTcoordSub)){ } 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); pubs_[ros_data_.coordSub]->publish(rosMsg);
} else if (!topic.compare(mqtt_data_.MQTTstateSub)){ } 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); pubs_[ros_data_.stateSub]->publish(rosMsg);
} else if (!topic.compare(mqtt_data_.MQTTinterfaceSub)) { } 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); pubs_[ros_data_.interfaceSub]->publish(rosMsg);
} else { } else {
ROS_ERROR("Topic NOT Implemented"); ROS_ERROR("Topic NOT Implemented");
@@ -66,49 +65,42 @@ class mqtt2rosClass: public virtual mqtt::callback {
/////////////// ROS CALLBACKS ////////// /////////////// ROS CALLBACKS //////////
//////////////////////////////////////// ////////////////////////////////////////
void commandCallback(const std_msgs::String::ConstPtr msg, mqtt::async_client* cli_, MQTT_topics* mqtttopics){ 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())); 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){ 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())); 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){ 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())); 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){ 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())); 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 ////////////////// //////////////// MAIN //////////////////
//////////////////////////////////////// ////////////////////////////////////////
int main(int argc, char **argv) { int main(int argc, char **argv) {
/// node variables /// /// node variables ///
int loopRate, msgBufferSize;
std::string param_ns; COMMON_parameters common_par;
MQTT_topics mqtt_topics; MQTT_topics mqtt_topics;
ROS_topics ros_topics;
std::string startup_msg_tmp;
std_msgs::String startup_msg;
/// setup node parameters /// /// setup node parameters ///
ros::init(argc, argv, "roboglue_relay"); ros::init(argc, argv, "roboglue_relay");
ros::NodeHandle nh; ros::NodeHandle nh;
///read parameters from server /// ///read parameters from server ///
nh.getParam("/roboglue_ros_relay/parameter_ns", param_ns); nh.getParam("/roboglue_ros_relay/parameter_ns", common_par.param_ns);
nh.getParam(param_ns+"startup_msg", startup_msg_tmp); loadCommonParameters(&nh, &common_par);
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/mqtt_INstate", mqtt_topics.MQTTstateSub); 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_INcommands", mqtt_topics.MQTTcommandSub);
nh.getParam("/roboglue_ros_relay/mqtt_INcoordinates", mqtt_topics.MQTTcoordSub); 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); nh.getParam("/roboglue_ros_relay/mqtt_qos", mqtt_topics.mqttQoS);
/// set spinner rate /// /// set spinner rate ///
ros::Rate loop_rate(loopRate); ros::Rate loop_rate(common_par.loopRate);
/// set console log level /// /// 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::Debug);
ROS_INFO("Relay Node Started"); ROS_INFO("Relay Node Started");
@@ -130,28 +122,31 @@ int main(int argc, char **argv) {
/// advertise publish topics /// /// advertise publish topics ///
publisherMap publishers; publisherMap publishers;
ros::Publisher command_pub = nh.advertise<std_msgs::String>(ros_topics.commandSub,static_cast<uint32_t>(msgBufferSize)); ros::Publisher command_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.commandSub,static_cast<uint32_t>(common_par.msgBufferSize));
ros::Publisher pose_pub = nh.advertise<std_msgs::String>(ros_topics.coordSub, static_cast<uint32_t>(msgBufferSize)); ros::Publisher pose_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.coordSub, static_cast<uint32_t>(common_par.msgBufferSize));
ros::Publisher state_pub = nh.advertise<std_msgs::String>(ros_topics.stateSub,static_cast<uint32_t>(msgBufferSize)); ros::Publisher state_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.stateSub,static_cast<uint32_t>(common_par.msgBufferSize));
ros::Publisher interface_pub = nh.advertise<std_msgs::String>(ros_topics.interfaceSub,static_cast<uint32_t>(msgBufferSize)); ros::Publisher interface_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.interfaceSub,static_cast<uint32_t>(common_par.msgBufferSize));
/// build a list of publisher to pass to mqtt callback /// /// build a list of publisher to pass to mqtt callback ///
publishers[command_pub.getTopic()] = &command_pub; publishers[command_pub.getTopic()] = &command_pub;
publishers[pose_pub.getTopic()] = &pose_pub; publishers[pose_pub.getTopic()] = &pose_pub;
publishers[state_pub.getTopic()] = &state_pub; publishers[state_pub.getTopic()] = &state_pub;
publishers[interface_pub.getTopic()] = &interface_pub; publishers[interface_pub.getTopic()] = &interface_pub;
/// create mqtt callback /// /// 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 /// /// subscribe to incoming topics ///
ros::Subscriber command_sub = nh.subscribe<std_msgs::String>(ros_topics.commandPub,static_cast<uint32_t>(msgBufferSize), ros::Subscriber command_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.commandPub,static_cast<uint32_t>(common_par.msgBufferSize),
boost::bind(commandCallback, _1, &client, &mqtt_topics)); boost::bind(commandCallback, _1, &client, &mqtt_topics));
ros::Subscriber pose_sub = nh.subscribe<std_msgs::String>(ros_topics.coordPub,static_cast<uint32_t>(msgBufferSize), ros::Subscriber pose_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.coordPub,static_cast<uint32_t>(common_par.msgBufferSize),
boost::bind(poseCallback, _1, &client, &mqtt_topics)); boost::bind(poseCallback, _1, &client, &mqtt_topics));
ros::Subscriber state_sub = nh.subscribe<std_msgs::String>(ros_topics.statePub,static_cast<uint32_t>(msgBufferSize), ros::Subscriber state_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.statePub,static_cast<uint32_t>(common_par.msgBufferSize),
boost::bind(stateCallback, _1, &client, &mqtt_topics)); boost::bind(stateCallback, _1, &client, &mqtt_topics));
ros::Subscriber interface_sub = nh.subscribe<std_msgs::String>(ros_topics.interfacePub,static_cast<uint32_t>(msgBufferSize), ros::Subscriber interface_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.interfacePub,static_cast<uint32_t>(common_par.msgBufferSize),
boost::bind(interfaceCallback, _1, &client, &mqtt_topics)); 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 /// /// CONNECT MQTT CLIENT ///
ROS_INFO("Connecting to MQTT..."); ROS_INFO("Connecting to MQTT...");
try { try {
@@ -171,8 +166,9 @@ int main(int argc, char **argv) {
while (ros::ok()) { while (ros::ok()) {
if (startCycle){ if (startCycle){
startCycle = false; startCycle = false;
startup_msg.data = startup_msg_tmp.c_str(); common_par.startupMsg.data = common_par.startupMsg.data + ros::this_node::getName();
publishers[command_pub.getTopic()]->publish(startup_msg); ROS_DEBUG("StartupMessage: %s", common_par.startupMsg.data.c_str());
publishers[interface_pub.getTopic()]->publish(common_par.startupMsg);
ros::spinOnce(); ros::spinOnce();
} }
ROS_DEBUG_ONCE("Relay Node Looping"); ROS_DEBUG_ONCE("Relay Node Looping");