I want to store in an array messages from a ROS topic for further elaboration
Hello, sorry if this question is too simple for you, but i don't have good programming skills and ROS knowledge.
I have a ROS topic in which are published some numbers that are heart beat intervals in seconds. I need to subscribe to that topic and do this kind of elaboration:
The idea is to have a little array of ten numbers in which i can store continuously ten heart beat. Then i have a bigger array of 60 numbers that must go up by ten position in order to have at the bottom the newest ten values of the small array and it has to "throw away" the ten oldest values ( i did a bit of research and maybe i have to use a vector instead of an array because in C++ array are fixed as far as i read ). Then i have to print every time these 60 values in a text file (i mean in a loop, so the the text file will be continuously overwritten).
Moreover, i see that ROS outputs the data from a topic in this manner: data: 0.678
with every data divided from the others by ---
in a column. What i really want, because i need it for a script that reads text file in this manner, is a text file in which the values are in one column without spaces and other signs or words, like this:
0.404
0.952
0.956
0.940
0.960
I provide below the code for my node, in which, for now, i did only the subscribing part, since i have no idea on how to do the things that i have to do later. Thank you in advance for your help!!!
Code:
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "../include/heart_rate_monitor/wfdb.h"
#include <stdio.h>
#include <sstream>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <algorithm>
#include <vector>
int main(int argc, char **argv)
{
ros::init(argc, argv, "writer");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("/HeartRateInterval", 1000);
ros::spin();
return 0;
}
NOTE: I didn't include the Float32/64 header because i publish the heart beats as a string. I don't know if this is of interest.
EDIT: Ok, with some good help of two guys over the web, now i have added a callback function that could be the one that i need and i called it into my subscriber. But, for now, i'm only able to create an empty file, data seem somewhat to be missing. Any advice? Thanks!
Code of the callback function that is called by the subscriber:
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "../include/heart_rate_monitor/wfdb.h"
#include <stdio.h>
#include <sstream>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <algorithm>
#include <deque>
using namespace std;
static std::deque<std::string> queue_buffer;
static int entries_added_since_last_write = 0;
std::ofstream data_file("/home/marco/catkin_ws/src/heart_rate_monitor/my_data_file.txt");
void write_data_to_file()
{
// open file
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();
}
void process_message(const std_msgs::String::ConstPtr& string_msg)
{
// 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;
}
}
The other code is the same.
EDIT_1:
I have included the code of the publisher if this can help. I've already tried to replace stringmsg called by processmessage in the subscriber node with msg, but with no luck. Thank you!
Code:
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "../include/heart_rate_monitor/wfdb.h"
#include <stdio.h>
#include <sstream>
#include <iostream>
#include <fstream>
#include <iomanip>
using namespace std;
int main(int argc, char **argv)
{
ros::init(argc, argv, "heart_rate_monitor");
ros::NodeHandle n;
ros::Publisher pub = n.advertise<std_msgs::String>("/HeartRateInterval", 1000);
ros::Rate loop_rate(1);
while (ros::ok())
{
ifstream inputFile("/home/marco/Scrivania/marks.txt");
string line;
while (getline(inputFile, line)) {
istringstream ss(line);
string heart;
ss >> heart;
std_msgs::String msg;
msg.data = ss.str();
pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
}
}
return 0;
}
UPDATE: I solved the problem! The program didn't enter in the second if of the process_message function because in the beginning the buffer of 60 numbers was empty and the double condition would never become true. So i simply changed the ==10
with >=10
. Thank you for your help!!
Asked by Marcofon on 2016-11-18 11:07:38 UTC
Comments
You need a callback function for the subscriber to enter each time there is a message received. In this callback function you can use the data.
Link
Asked by JoshMarino on 2016-11-18 22:43:51 UTC
@JoshMarino thank you for your reply! Can you explain me in details how to do it? Thank you again!
Asked by Marcofon on 2016-11-20 09:58:55 UTC
Have you tried following the tutorial in the link I provides for subscribers in C++?
Asked by JoshMarino on 2016-11-20 12:17:27 UTC
@JoshMarino hello, I'm sorry. Which links? EDIT: check the edit in the question. Do you have an idea of what is wrong or missing? Thank you!
Asked by Marcofon on 2016-11-20 13:31:53 UTC
I have added also the publisher code.
Asked by Marcofon on 2016-11-22 06:14:41 UTC
I solved the problem!
Asked by Marcofon on 2016-11-22 09:47:29 UTC