rosserial doesnot publish imu data when combined with some other data publisher/subscriber

asked 2019-05-06 07:48:35 -0600

pravin1 gravatar image

I am trying to publish imu_data, wheel_velocities as well as to subscribe to cmd_vel, and some other topics using rosserial_arduino. I am using Arduino Mega 2560 and UM7 is my IMU. When i use only imu and publish its data, it publishes correctly but when I combine it with some other publishers, it does not publish imu_data at all but other data are getting published.

Below is a test program(not my original program) in which i am trying to publish imu_data and random_number.

#include<ros.h> // need this preprocessor directory for ros
#include <UM7.h>  // um7 arduino binary parser
#include <sensor_msgs/Imu.h> // need this for generating imu messages in ros
#include <std_msgs/Float32.h>

ros::NodeHandle nh;

UM7 imu;
sensor_msgs::Imu imu_msg;
ros::Publisher imu_data("/imu_data", &imu_msg);

std_msgs::Float32 random_number;
ros::Publisher pub_rand("random_number", &random_number);

void setup() {
   nh.getHardware()->setBaud(57600); // setting the baud rate for rosserial
   nh.initNode();

  // put your setup code here, to run once:
  Serial.begin(57600);
  Serial2.begin(57600);
  nh.advertise(imu_data);

  randomSeed(analogRead(0));

  nh.advertise(pub_rand); 
}

void PublishImuData()
{

     if (Serial2.available() > 0) {
        if (imu.encode(Serial2.read())) {  // Reads byte from buffer.  Valid packet returns true.
            imu_msg.header.frame_id= "/imu";
            //orientation
        imu_msg.orientation.w = imu.quat_A/29789.09091;
        imu_msg.orientation.x = imu.quat_B/29789.09091;
        imu_msg.orientation.y = imu.quat_C/29789.09091;
        imu_msg.orientation.z = imu.quat_D/29789.09091;

        // angular velocity
        imu_msg.angular_velocity.x = imu.vel_x;
        imu_msg.angular_velocity.y = imu.vel_y;
        imu_msg.angular_velocity.z = imu.vel_z;

        // linear acceleration
        imu_msg.linear_acceleration.x = imu.acc_x;
        imu_msg.linear_acceleration.y = imu.acc_y;
        imu_msg.linear_acceleration.z = imu.acc_z;

        imu_data.publish(&imu_msg); // publishing the imu data        
    }
  }
}

void loop() {  


  // put your main code here, to run repeatedly:
 PublishImuData();
 nh.spinOnce();


 random_number.data= random(10,300);
 pub_rand.publish(&random_number);
 nh.spinOnce();


}

Below is the output when i run the above program:

rosrun rosserial_arduino serial_node.py _port:=/dev/ttyACM0
[INFO] [1557145262.008324]: ROS Serial Python Node
[INFO] [1557145262.014604]: Connecting to /dev/ttyACM0 at 57600 baud
[INFO] [1557145264.122755]: Requesting topics...
[INFO] [1557145264.171944]: Note: publish buffer size is 1024 bytes
[INFO] [1557145264.172282]: Setup publisher on /imu_data [sensor_msgs/Imu]
[INFO] [1557145264.176375]: Setup publisher on random_number [std_msgs/Float32]

rostopic list shows me this:

rostopic list
/diagnostics
/imu_data
/random_number
/rosout
/rosout_agg

output of rostopic hz /imu_data

    rostopic hz /imu_data 
    subscribed to [/imu_data]
    no new messages
    no new messages
    ^Cno new messages

output of rostopic hz /random_number

rostopic hz /random_number 
subscribed to [/random_number]
average rate: 490.636
    min: 0.001s max: 0.004s std dev: 0.00048s window: 466
^Caverage rate: 490.154
    min: 0.001s max: 0.004s std dev: 0.00049s window: 939
edit retag flag offensive close merge delete

Comments

I found that if i change if (Serial2.available() > 0) to while(Serial2.available() > 0) , then it started to work. But now the problem is publish rate of other data are reduced significantly. Any idea, how to fix this problem? In my real problem, some times rostopic hz /rpm_values for example leads to No new messages

pravin1 gravatar imagepravin1 ( 2019-05-08 07:59:48 -0600 )edit

Also the rosrun rosserial_arduino serial_node.py _port:=/dev/ttyACM0 gives [ERROR] [1557321844.680810]: Lost sync with device, restarting...

pravin1 gravatar imagepravin1 ( 2019-05-08 08:24:28 -0600 )edit