rosserial doesnot publish imu data when combined with some other data publisher/subscriber
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
I found that if i change
if (Serial2.available() > 0)
towhile(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 timesrostopic hz /rpm_values
for example leads toNo new messages
Also the
rosrun rosserial_arduino serial_node.py _port:=/dev/ttyACM0
gives[ERROR] [1557321844.680810]: Lost sync with device, restarting...