Hi to all,
I have a ROS node which publish data coming from encoders and current sensors (the node outputs velocity data, raw encoders ticks and currents).
Currently, my ROS node publish all these data as std_msgs/String, but this gives me lot of problems when I need to handle data.
I would like to publish a structured ROS message which reports the robot status; what should I use for encoders ticks, velocity values and currents?
How should I re-write the code in order to implement a correct structured message? I think the best thing to do is to publish a topic for the velocity, another one for the motor current and another for the encoder ticks.
This is my current code, as you can see I declare everything like std_msgs::String.
Thank you for your help.
while (ros::ok())
{
std_msgs::String msg_sys, msg_enc, msg_io, msg_vel;
std:string temp="", volt="", amotor="", enco="", aio="", dio="";
std::stringstream ss, ss2, ss3, ssvel;
if (sys == 1){
device.GetValue(_T, 0, temp);
device.GetValue(_V, 0, volt);
device.GetValue(_A, amotor);
if (temp != "" && volt != "" && amotor != ""){
std::replace(temp.begin(), temp.end(), ':', ',');
std::replace(volt.begin(), volt.end(), ':', ',');
std::replace(amotor.begin(), amotor.end(), ':', ',');
ros::Time time = ros::Time::now();
ss << time << "," << temp << "," << volt << "," << amotor << "," << count;
msg_sys.data = ss.str();
sys_pub.publish(msg_sys);
}
}
//Encoder Topic
if (enc == 1){
current_time =ros::Time::now().toSec();
device.GetValue(_C, 0, enco);
//enco ="15000:16000";
if (enco != ""){
//Velocity
if (velocity == 1){
std::string vel_enco=enco;
std::replace(vel_enco.begin(), vel_enco.end(), ':', ' ');
const char * c_enco = vel_enco.c_str();
char* pEnd;
double Encoder_sx, Encoder_dx;
double Distance_sx, Distance_dx, Distance_prev_sx, Distance_prev_dx, Vel_sx, Vel_dx;
float Vel_linear, w;
Encoder_sx = std::strtod (c_enco,&pEnd);
Encoder_dx = std::strtod (pEnd,&pEnd);
Encoder_sx=Encoder_sx/pulse_rev_left;
Encoder_dx=Encoder_dx/pulse_rev_right;
Distance_sx = (Encoder_sx)*sprocket;
Distance_dx = (Encoder_dx)*sprocket;
last_time =ros::Time::now().toSec();
Vel_sx = ((Distance_prev_sx - Distance_sx) / (last_time - current_time))/1000; // m / sec
Vel_dx = ((Distance_prev_dx - Distance_dx) / (last_time - current_time))/1000;
Distance_prev_sx = Distance_sx;
Distance_prev_dx = Distance_dx;
Vel_linear = (Vel_dx + Vel_sx)/2;
w = (Vel_dx - Vel_sx)/track;
ros::Time time2 = ros::Time::now();
//Publish velocity topic
ssvel << time2 << "," << Distance_sx << "," << Distance_dx << "," << Vel_sx << "," << Vel_dx << "," << Vel_linear << "," << w;
msg_vel.data = ssvel.str();
vel_pub.publish(msg_vel);
}
ros::Time time3 = ros::Time::now();
std::replace(enco.begin(), enco.end(), ':', ',');
//Publish enco topic
ss2 << time3 << "," << enco << "," << count;
msg_enc.data = ss2.str();
enc_pub.publish(msg_enc);
}
}
Originally posted by Marcus Barnet on ROS Answers with karma: 287 on 2018-03-22
Post score: 1
Original comments
Comment by gvdhoorn on 2018-03-22:
To take this question a bit broader: do you want to stay at the abstraction level of "ticks" and "encoders", or would it make sense to have your node(s) publish more semantically meaningful messages with fields in SI units?
That could be done by the 'next' node btw, but is something to consider.
Comment by Marcus Barnet on 2018-03-22:
This would be even better, but also the absolute values are OK for me.
For example, having something like:
left_ticks: 1234
right_ticks: 2345
works fine for me; what I would like to obtain is to output the values as integers and floats so I can plot them in rqt_plot or use them for other purposes.
Comment by Marcus Barnet on 2018-03-22:
At the moment, the std_msgs::String format is giving me lots of problems when I have to handle the node output.