lthul16's profile - activity

I am trying to publish the data being sent through a serial port to sensor_msgs/imu. The data reads in for each axes of the accelerometer and gyroscope in one line and then the line is split into the six values and converted to a float. It is then saved into the message. The problem i am having is when I change the rate of the loop it either causes a delay between displaying and actually moving the IMU or it starts to chop data off. I have tried synchronizing the frequency it is being sent to the loop rate, but the data does not come through the serial port at a constant rate.

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "sensor_msgs/Imu.h"
#include "serial/serial.h"
#include <sstream>
#include <rosbag/bag.h>

int main(int argc, char **argv)
{

  ros::init(argc, argv, "Imu_talker");


  ros::NodeHandle n;


  ros::Publisher chatter_pub = n.advertise<sensor_msgs::Imu>("imu/data_raw", 1000);

  rosbag::Bag Imu_bag("Imu_bag.bag", rosbag::bagmode::Write);


  ros::Rate loop_rate(31.5);
  std::string port = "/dev/ttyACM0";
  uint32_t baudrate = 115200;
  serial::Serial mySerial;
  mySerial.setPort(port);
  mySerial.setBaudrate(baudrate);
  mySerial.open();

  int count = 0;
  while (ros::ok())
  {

sensor_msgs::Imu imu_msg;

imu_msg.header.stamp = ros::Time::now();
imu_msg.header.frame_id = "/base_link";

std::string line;
line = mySerial.readline(1024, "\n");
mySerial.flush();
ROS_INFO("%s", line.c_str());
char *pch;
pch = std::strtok(&line[0], " ");
std::string number;
double data[6] = {0};
int count = 0;
while(pch != NULL)
{
    number = pch;
    data[count] = atof(number.c_str());
    pch = std::strtok(NULL, " ");
    count++;
}     

float angvel_conversion = .005*3.1415/180.0;
float linacc_conversion = .00025*9.8;

imu_msg.angular_velocity.x = data[0]*angvel_conversion;
imu_msg.angular_velocity.y = data[1]*angvel_conversion;
imu_msg.angular_velocity.z = data[2]*angvel_conversion;
imu_msg.linear_acceleration.x = data[3]*linacc_conversion;
imu_msg.linear_acceleration.y = data[4]*linacc_conversion;
imu_msg.linear_acceleration.z = data[5]*linacc_conversion;

chatter_pub.publish(imu_msg);
Imu_bag.write("Imu", ros::Time::now(), imu_msg);

ros::spinOnce();

loop_rate.sleep();

 }

 Imu_bag.close();
 return 0;
 }

The output should have 6 numbers per line, but instead it will cut the last number off the line sometimes or skip the line entirely.

[ INFO] [1471983854.836844912]: -14 2 11 -57 12 3988 

[ INFO] [1471983854.861861641]: -18 1 25 -42 21 4004 

[ INFO] [1471983854.886845614]: -14 51 -10 -32 9 3922 

[ INFO] [1471983854.911821346]: -2 -24 6 -32 -19
[ INFO] [1471983854.937312646]:  4015 

[ INFO] [1471983854.961860879]: -10 0 -26 -21 44 3998 

[ INFO] [1471983854.986839265]: -14 0 4 -21 21 3975 

[ INFO] [1471983855.011831759]: 3 13 3 -41 -13 4009 

[ INFO] [1471983855.036831097]: -7 32 9 -13 27 4021 

[ INFO] [1471983855.061776239]: 
[ INFO] [1471983855.086830751]: -4 26 6 -41 -10 3988