Search code examples
c++arraystextmessageros

Is there a way to have a precedence between two ROS nodes?

I'm asking you if there is a way to have a precedence between two ROS nodes. In particular, i have a ROS node that makes an output that is a text file with 60 data in it, and it recreates it every time because the data are changing. Then i have a node that has to analyze that text file. Basically, what i need is to make some changes to have a mechanism that stops the analyzer node when the writer node is running, then it has to send a signal to the analyzer node to make it able to run and analyze the text file. And then the writer node has to return let's say "in charge" to be able to re-write the text file again. So, in simple words, is a loop. Someone told me that a possible solution can be something like a "semaphore" topic in which the writer node writes, for example, a boolean value of 1 when is doing the opening, the writing and the closing of the text file, so the analyzer node knows that cannot do its elaboration, since the file is not ready yet. And, when the writer has finished and closed the text file, it has to be published a value 0 that permits the analysis by the analyzer node. I searched for the publishing of boolean values and i found a code that can be something like this:

ros::Publisher pub = n.advertise<std_msgs::Bool>("semaphore", 1000);
std_msgs::Bool state;
state.data = 1;

I don't know if i have only to use the publisher in the writer node and the subscriber in the analyzer node. Maybe i have to use both of them in the two nodes, something like: writer put a 1 in the topic semaphore so the analyzer knows that cannot access the text file, makes the text file and then put a 0 in the topic and subscribe to the topic waiting again a 1; the analyzer does something similar but in reverse. I put the two codes below, because i don't have any idea where to put the publisher and the the subscriber and how to make them working well. If possible, i have to keep this structure of working flow in my codes. NOTE: a new text file is created almost every 10 seconds, since in the text file are written data coming from another ROS topic and the code in the writer has a mechanism to do this kind of elaboration. Thank you in advance!!! EDIT: Now the codes are corrected with a topic based solution as i explain in my last comment.

Writer code:

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Bool.h"
#include "../include/heart_rate_monitor/wfdb.h"
#include <stdio.h>
#include <sstream>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <algorithm>
#include <deque>
#include "heart_rate_monitor/analyse_heart_rate.h"

using namespace std;



static std::deque<std::string> queue_buffer;
static int entries_added_since_last_write = 0;

ros::Publisher pub;

void write_data_to_file()
{
// open file;
std::ofstream data_file("/home/marco/catkin_ws/src/heart_rate_monitor/my_data_file.txt");
if (data_file.is_open())
{
for (int i = 0; i < queue_buffer.size(); ++i)
{
  data_file << queue_buffer[i] << std::endl;
}
}
else
{
std::cout << "Error - Cannot open file." << std::endl;
exit(1);
}
data_file.close();

std_msgs::Bool state;
state.data = 0;

pub.publish(state);

}

void process_message(const std_msgs::String::ConstPtr& string_msg)
{
std_msgs::Bool state;
state.data = 1;

pub.publish(state);

// if buffer has already 60 entries, throw away the oldest one
if (queue_buffer.size() == 60)
{
queue_buffer.pop_front();
}

// add the new data at the end
queue_buffer.push_back(string_msg->data);

// check if 10 elements have been added and write to file if so
entries_added_since_last_write++;

if (entries_added_since_last_write >= 10
  && queue_buffer.size() == 60)
{
// write data to file and reset counter
write_data_to_file();
entries_added_since_last_write = 0;
}

}


int main(int argc, char **argv)
{

ros::init(argc, argv, "writer");

ros::NodeHandle n;

ros::Subscriber sub = n.subscribe("/HeartRateInterval", 1000, process_message);
pub = n.advertise<std_msgs::Bool>("/semaphore", 1000);

ros::spin();

return 0;
}

Analyzer code:

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Bool.h"
#include "../include/heart_rate_monitor/wfdb.h"
#include <stdio.h>
#include <sstream>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <algorithm>
#include <deque>
#include "heart_rate_monitor/analyse_heart_rate.h"

void orderCallback(const std_msgs::Bool::ConstPtr& msg)
{

if (msg->data == 0)
{
chdir("/home/marco/catkin_ws/src/heart_rate_monitor");

system("get_hrv -R my_data_file.txt >doc.txt");
}
}


int main(int argc, char **argv)
{

ros::init(argc, argv, "analyzer");

ros::NodeHandle n;

ros::Subscriber sub = n.subscribe("/semaphore", 1000, orderCallback);

ros::spin();

return 0;
}
Solution

  • This could be done simply using ROS services. Basically, when your node A gets the message, it does what it needs (write file) and then asks for a serice from node B (analyse the file).

    The only con I see is that node A will have to wait for node B service to finish. If B dosen't need too much time, it wouldn't raise a problem.

    Code Snippet :

    Srv :

    create a service named "analyse_heart_rate.srv" in the srv folder of your package (I supposed it's name "heart_rate_monitor").

    specify the request/response of your service structure in the file:

    string filename
---
bool result

    CMakeLists :

    add the following lines :

    add_service_files(
  FILES
  analyse_heart_rate.srv
)

    Service Server :

     #include "ros/ros.h"
 #include "heart_rate_monitor/analyse_heart_rate.h"


bool analyse(heart_rate_monitor::analyse_heart_rate::Request  &req,
     heart_rate_monitor::analyse_heart_rate::Response &res)

{
  res.result = analyse_text_file(req.filename);
  return true;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "heart_rate_analyser_server");
  ros::NodeHandle n;

  ros::ServiceServer service = n.advertiseService("heart_rate_analyser", analyse);
  ROS_INFO("Ready to analyse requests.");
  ros::spin();

  return 0;
}

    Service Client

    #include "ros/ros.h"
#include "heart_rate_monitor/analyse_heart_rate.h"

void process_message(const std_msgs::String::ConstPtr& string_msg)
{
    std::string output_filename;
    do_staff_with_message();
    write_data_to_file_(output_filename);

     heart_rate_monitor::analyse_heart_rate srv;
     srv.filename = output_filename ;
     if (client.call(srv))
     {
        ROS_INFO("Result: %d", (bool)srv.response.result);
     }
     else
     {
        ROS_ERROR("Failed to call service heart_rate_analyser");
     }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "add_two_ints_client");
  ros::NodeHandle n;
  ros::ServiceClient client = n.serviceClient<heart_rate_monitor::analyse_heart_rate>("heart_rate_analyser");
  ros::Subscriber sub = n.subscribe("/HeartRateInterval", 1000, process_message);

  return 0;
}

    This way, whenever a message comes in the node "Service Client", It will process it and eventually write it to the file. Then it asks the "Service Server" to process the file created previously...

    Of course, this is just a snippet, costumize it to your needs.

    Cheers.