Commit 83651396 authored by Victor Lamoine's avatar Victor Lamoine
Browse files

Allow to choose: number of data per line, publish topic.

parent 7e0a0b91
Pipeline #174113788 canceled with stage
in 59 seconds
......@@ -16,7 +16,7 @@ class SerialPortReader : public QObject
Q_OBJECT
public:
SerialPortReader(QSerialPort *serial_port, QObject *parent = 0);
SerialPortReader(QSerialPort *serial_port, const unsigned number_of_data, QObject *parent = 0);
~SerialPortReader();
std::shared_ptr<std::vector<ros::Publisher>> publishers_;
......@@ -31,6 +31,7 @@ private Q_SLOTS:
void publishTemperatures(QString line, QDateTime date_time);
private:
unsigned number_of_data_;
QSerialPort *serial_port_;
QByteArray data_read_;
};
......
......@@ -9,22 +9,31 @@ using std::endl;
int main(int argc, char *argv[])
{
ros::init(argc, argv, "serial_temperatures");
std::shared_ptr<ros::NodeHandle> nh(std::make_shared<ros::NodeHandle>());
std::shared_ptr<ros::NodeHandle> nh(std::make_shared<ros::NodeHandle>("~"));
int data_per_line(1);
nh->getParam("data_per_line", data_per_line);
if (data_per_line < 1)
{
ROS_ERROR_STREAM("data_per_line cannot be < 1");
return 1;
}
ROS_INFO_STREAM("Data per line = " << data_per_line);
std::string topic;
nh->getParam("topic", topic);
ros::AsyncSpinner s(2);
s.start();
std::shared_ptr<std::vector<ros::Publisher>> publishers;
publishers = std::make_shared<std::vector<ros::Publisher>>();
publishers->resize(6);
publishers->at(0) = nh->advertise<std_msgs::Float32>("/ram/sensors/thermocouples/0", 5);
publishers->at(1) = nh->advertise<std_msgs::Float32>("/ram/sensors/thermocouples/1", 5);
publishers->at(2) = nh->advertise<std_msgs::Float32>("/ram/sensors/thermocouples/2", 5);
publishers->at(3) = nh->advertise<std_msgs::Float32>("/ram/sensors/thermocouples/3", 5);
publishers->at(4) = nh->advertise<std_msgs::Float32>("/ram/sensors/thermocouples/4", 5);
publishers->at(5) = nh->advertise<std_msgs::Float32>("/ram/sensors/thermocouples/5", 5);
publishers->resize(data_per_line);
for (std::size_t i(0); i < publishers->size(); ++i)
publishers->at(i) = nh->advertise<std_msgs::Float32>(topic + std::to_string(i), 5);
QCoreApplication app(argc, argv);
QSerialPort *serial_port(new QSerialPort);
SerialPortReader *serial_port_reader(new SerialPortReader(serial_port));
SerialPortReader *serial_port_reader(new SerialPortReader(serial_port, data_per_line));
serial_port_reader->publishers_ = publishers;
serial_port_reader->nh_ = nh;
......@@ -33,6 +42,9 @@ int main(int argc, char *argv[])
int baudrate(115200);
nh->getParam("baudrate", baudrate);
ROS_INFO_STREAM("Port = " << port);
ROS_INFO_STREAM("Baudrate = " << baudrate);
serial_port->setPortName(port.c_str());
serial_port->setBaudRate(baudrate);
if (!serial_port->open(QIODevice::ReadOnly))
......
#include <serial_temperatures/serialportreader.hpp>
SerialPortReader::SerialPortReader(QSerialPort *serial_port,
const unsigned number_of_data,
QObject *parent) :
QObject(parent),
number_of_data_(number_of_data),
serial_port_(serial_port)
{
connect(serial_port, &QSerialPort::readyRead, this, &SerialPortReader::readData);
......@@ -59,14 +61,14 @@ void SerialPortReader::publishTemperatures(QString line, QDateTime)
QRegExp rx("[;]");
QStringList list = line.split(rx, QString::SkipEmptyParts);
if (list.size() != 6)
if (list.size() != (int) number_of_data_)
{
ROS_WARN_STREAM("Wrong message received. Expected 6, got " << list.size());
ROS_WARN_STREAM("Wrong message received. Expected " << number_of_data_ <<", got " << list.size());
return;
}
std_msgs::Float32 msg;
for (int i(0); i < 6; ++i)
for (unsigned i(0); i < number_of_data_; ++i)
{
bool ok;
double value(list.at(i).toDouble(&ok)); // Check that conversion is successful
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment