Sync lost when publishing Odometry message.

[AlexisTM]

Dear,

• Hardware : Mega2560
• Software : Indigo, 0.6.4

I keep having Sync issues when I am trying to send the nav_msgs/Odometry message.

I have multiple other topics, LEDs subscriber to a std_msgs/ColorRGBA topic, US sensors (non blocking architecture with NewPing library) publishing a std_msgs/Bool topic. All those are working if I do not send the Odometry message (commenting off rover_odom.publish(&odometry_msg);) but if I uncomment it, simply loose Sync all the time.

Is the message too big?

boolean odometrySender(){
    analogWrite(LED_R_PIN, 255);
    analogWrite(LED_G_PIN, 0);
    analogWrite(LED_B_PIN, 0);
    now_encoder_time = millis()/1000.0;
    if(!updated_left) return false;
    if(!updated_right) return false;

    delta_left = current_encoder_left - last_encoder_left;
    delta_right = current_encoder_right - last_encoder_right;

    omega_left = (delta_left * distance_per_tick) / (now_encoder_time - last_encoder_time);
    omega_right = (delta_right * distance_per_tick) / (now_encoder_time - last_encoder_time);

    v_left = omega_left * WHEEL_RADIUS; //radius
    v_right = omega_right * WHEEL_RADIUS;

    vx = ((v_right + v_left) / 2)*10; // Times 10 ?
    vy = 0;
    vth = ((v_right - v_left)/distance_between_wheels)*10;

    double dt = (now_encoder_time - last_encoder_time);
    double delta_x = (vx * cos(th)) * dt;
    double delta_y = (vx * sin(th)) * dt;
    double delta_th = vth * dt;

    x += delta_x;
    y += delta_y;
    th += delta_th;

    //geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
/*
    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = ros_now_encoder_time;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_link";

    odom_trans.transform.translation.x = x;
    odom_trans.transform.translation.y = y;
    odom_trans.transform.translation.z = 0.0;
    //odom_trans.transform.rotation = odom_quat;

    //send the transform
    odom_broadcaster.sendTransform(odom_trans);
*/
    //Odometry message

    odometry_msg.header.stamp = nh.now();
    odometry_msg.header.frame_id = "odom";

    //set the position
    odometry_msg.pose.pose.position.x = x;
    odometry_msg.pose.pose.position.y = y;
    odometry_msg.pose.pose.position.z = 0.0;
    //odometry_msg.pose.pose.orientation = quat;

     //set the velocity
    odometry_msg.child_frame_id = "base_link";
    odometry_msg.twist.twist.linear.x = vx;
    odometry_msg.twist.twist.linear.y = vy;
    odometry_msg.twist.twist.angular.z = vth;

    //publish the message
    rover_odom.publish(&odometry_msg);

    last_encoder_left = current_encoder_left;
    last_encoder_right = current_encoder_right;
    last_encoder_time = now_encoder_time;
    updated_left = false;
    updated_right = false;
    analogWrite(LED_R_PIN, 0);
    analogWrite(LED_G_PIN, 0);
    analogWrite(LED_B_PIN, 255);
}

[AlexisTM]

I guess the message is just way too big. It is more than 700 bytes (576 only for covariances in Float64)

[attermann]

You should be able to broadcast odometry no problem. Check your baud rate. Do a rough calculation of baud based on your message size and frequency.

…

------ Original Message ------ From: "Alexis Paques" <notifications@github.com> To: "ros-drivers/rosserial" <rosserial@noreply.github.com> Cc: "Subscribed" <subscribed@noreply.github.com> Sent: 3/9/2017 8:42:18 AM Subject: Re: [ros-drivers/rosserial] Sync lost when publishing Odometry message. (#277)

I guess the message is just way too big. It is more than 700 bytes (576 only for covariances in Float64) — You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub, or mute the thread.

[AlexisTM]

Over 700 bytes, around 6kbits (1Hz) on a 57kbits channel. Even with 1Hz, it lose the sync when it should send the Odometry message. Sometimes I receive it and loose sync, and sometimes I simply loose the sync before getting the message. My second guess is the output buffer size.

My solution will be to use the much shorter custom message, and make the covariance computation on the ros embedded computer instead.

[Galdeano]

Maybe you can just increase the buffer size:
http://wiki.ros.org/rosserial/Overview/Limitations
With something like this:
http://answers.ros.org/question/125451/how-to-publish-a-array-msgs-with-large-data-size-in-arduino/

[AlexisTM]

Sure, but the size of the message is restrictive while most of the informations are unused.

• The Arduino code uses 32 bits logic ; The Odometry message is using 64 bit logic for every data
• We only use the diagonal (6*2*8 bytes) of the covariance field (2*36*8 bytes)

[Galdeano]

that true, making custom message for arduino and nice and well formatted message on the ros computer is often the best solution.

[AlexisTM]

This works perfectly with the custom message @20hz

# Sending encoders values for further computation
Header header
int32 left
int32 right
int32 dleft
int32 dright
float32 dt
float32 x
float32 y
float32 vx
float32 vy
float32 th
float32 vth

[attermann]

I ultimately opted for custom messages as well, and I use a node on the ROS PC as a "proxy" to expand the custom messages back into full messages and re-publish them. I send both odom and imu mesages with covariances at 30Hz each without any issue. For covariances, I send only the diagonals as 32-bit in the custom messages. If you're interested I'm happy to share my code.

[AlexisTM]

@atterman I would be very grateful, so I would have much less the pain of rewriting the messages :D

[attermann]

Here you go: https://github.com/ecostech/rosproxy.

I had to extract it from a larger project, so forgive any untidiness, but it should build in catkin_make.

There is example Arduino code in rosproxy_arduino/sketchbook, and the Arduino headers you will need to copy to ros_lib are in rosproxy_arduino/sketchbook/libraries/ros_lib/rosproxy_msgs. These headers aren't re-built by catkin yet, so if you change the custom msgs for any reason then you will need to re-build ros_lib manually.

Cheers.

[AlexisTM]

Thanks a lot!
It is still pretty tidy! Note that I always rebuild the ros_lib libraries so I can be sure I have the compatible compiled version :p