Write/Improve a ROS node to publish encoder data
Hi to all,
I'm using two incremental encoders (SICK DFS60B) on my skid steering robot. A full wheel turn on each side generates about 2241114 pulses.
I wrote a ROS node in C language in order to publish the encoders data and the wheel velocity.
The node works well, but the problem is that the encoders data are too big.
Is there any ROS package or library which can properly handle the encoders data?
What can I do to improve the encoder topic and my code?
Is there any better method to publish odometry data?
EDIT 1: it seems that the motor controller that I'm using to read the encoder pulses cannot handle the current PPR. I'm going to decrease the PPR for my encoders and check if the problem goes away. In the meanwhile, how do you think I can improve my code? Is there anything I can change? Is there any ROS library which I can include in my code to optimize the velocity calculation?
int main(int argc, char **argv)
{
ros::init(argc, argv, "robot_base");
ros::NodeHandle n;
std::string port = "";
int sys=0, enc=0, io=0, velocity=0, rate=10;
n.getParam("/robot_base/port", port);
n.getParam("/robot_base/sys", sys);
n.getParam("/robot_base/enc", enc);
n.getParam("/robot_base/io", io);
n.getParam("/robot_base/velocity", velocity);
n.getParam("/robot_base/rate", rate);
//Debug
//ROS_INFO("%d - %d - %d - %s", sys, enc, io, port.c_str());
long long unsigned int count = 0; //Frame counter
RoboteqDevice device;
int status = device.Connect(port.c_str());
ros::Publisher sys_pub = n.advertise<std_msgs::String>("robot/sys", 1);
ros::Publisher enc_pub = n.advertise<std_msgs::String>("robot/enc", 1);
ros::Publisher vel_pub = n.advertise<std_msgs::String>("robot/velocity", 1);
ros::Publisher io_pub = n.advertise<std_msgs::String>("robot/io", 1);
ros::Subscriber cmd_sub = n.subscribe("robot/cmd_vel", 1, &RoboteqDevice::cmdCallback, &device);
double current_time =ros::Time::now().toSec();
double last_time =ros::Time::now().toSec();
ros::Rate loop_rate(rate);
while (ros::ok())
{
std_msgs::String msg_sys, msg_enc, msg_io, msg_vel;
std:string temp="", volt="", amotor="", enco="", aio="", dio=""; //Strings that receives data from roboteq device
std::stringstream ss, ss2, ss3, ssvel; //Strings used to build topics message
//System Topic
if (sys == 1){
device.GetValue(_T, 0, temp);
device.GetValue(_V, 0, volt);
device.GetValue(_A, amotor);
//printf("Debug: %s -- %llu \n", temp.c_str(), count);
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.header.stamp = ros::Time::now();
msg_sys.data = ss.str();
//ROS_INFO("%s", msg_sys.data.c_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){
//Convert encoder string in long long integer
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 ...
Hi, can you be more specific ? Is it a problem with the size of the data (memory size) or the latency (Low rate of publishing) or computing speed (CPU)?
for the first case, try Boost.Multiprecision
for the second, try nodelet if you can.
Or if this is a CPU problem, maybe try GPU computing ?
At high speed and for distances greater than 8 meters, the encoder data seem to saturate in some way. On the contrary, at low speed, everything works great. I cannot understand if it is an encoder problem or a problem with my code. The input velocity is 2200RPM at max at high speed.
I use a good CPU, so i do not think it's a CPU problem. I cannot understand if it is related to my encoders which cannot handle high speed or to my code which is not optimized.
Sounds like a sampling rate issue, how often do you sample the inputs and at what rate do you expect your 2241114 pulses to change?
It seems that the motor controller that I'm using to read the pulses cannot handle the current PPR. I'm going to decrease the PPR for my encoders and check if the problem goes away. In the meanwhile, how do you think I can improve my code? Is there anything I can change?