ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Error getting Imu via ros serial

asked 2023-01-12 01:51:40 -0500

daidalos99 gravatar image

Hi guys. This question might be foolish to somebody, but I'm gonna post it anyway. I'm trying to get Imu value via ros serial and the composition of value is like below.

channel-id, quaternion(z y x w), gyroscope(x y z), accelerometer(x y z), Distance(x y z), battery(%) -->> in total, 15 datas. example: 100-0,0.1469,0.0001,-0.0056,0.9891,0.0,0.0,0.0,0.002,-0.014,1.000,0.000,0.000,68

and I tried to get a value via ros serial with delimiter ',', put in the vector 'result'. And I printed the size after printed the element of 'result' vector.

Baudrate is 115200, and my ros loop rate is 1000 for now. Here's the question. By some reason, I cannot get the full 15 data. While getting 11th data, it cutted off and (including the back part of cutted of data) 5 left prints to next time stamp. And I tried to do everything I can(changing baud rate, loop rate etc) but it doesn't figure out the problem. When I changed the loop rate, the size of result vector changes, but it's not 15(that I wanted to be).

the code is like below..originally it's a code that put imu data to sensor but commented it all out to just to check the data that I publish and its number(result.size()). If any knows how to figure out, pls help me.

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/Quaternion.h>
#include <serial/serial.h>
#include <std_msgs/Float64.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf/transform_broadcaster.h>
#include <iostream>
#include <sstream>
#include <string>
#include <typeinfo>

using namespace std;
serial::Serial ser;
int publish_rate = 100;

vector<string> split(std_msgs::String str, char Delimiter) {
    istringstream iss(str.data);
    string buffer; 
    vector<string> result = {};

    while (getline(iss, buffer, Delimiter)) {
        cout << buffer << ", ";
        result.push_back(buffer); 
    }

    return result;
}

void write_callback(const std_msgs::String::ConstPtr& msg){
    ROS_INFO_STREAM("Writing to serial port" << msg->data);
    ser.write(msg->data);
}

int main(int argc, char **argv)
{
   ros::init(argc, argv, "imu"); // 노드이름 imu
   ros::NodeHandle nh;
   ros::Subscriber imu_sub = nh.subscribe("write", publish_rate, write_callback);
   ros::Publisher imu_pub = nh.advertise<sensor_msgs::Imu>("imu_data", publish_rate);

   tf2::Quaternion q;
   static tf2_ros::TransformBroadcaster br;
   geometry_msgs::TransformStamped transformStamped;

   transformStamped.header.stamp = ros::Time(0);
   transformStamped.header.frame_id = "map";
   transformStamped.child_frame_id = "imu_link";

   q.setRPY(0, 0, 0);

   try{
      ser.setPort("/dev/ttyUSB0");
      ser.setBaudrate(115200);
      serial::Timeout to = serial::Timeout::simpleTimeout(publish_rate);
      ser.setTimeout(to);
      ser.open();
  }

  catch(serial::IOException& e){
      ROS_ERROR_STREAM("Unable to open port ");
      return -1;
  }

  if(ser.isOpen()){
        ROS_INFO_STREAM("Serial Port initialized");
  }
  else{
      return -1;
  }

  ros::Rate loop_rate(publish_rate);
  std_msgs::String sensor;
  sensor_msgs::Imu imu;
  imu.header.frame_id = "imu_link";

  int cnt = 0;

  while (ros::ok()){
    ros::spinOnce();
    if(ser.available()){
      ROS_INFO_STREAM("Reading from serial port");

      imu.header.seq = cnt; 
      transformStamped.header.seq = cnt;
      sensor ...
(more)
edit retag flag offensive close merge delete

Comments

Please provide a link to the API for "serial/serial.h". For a serial port, usually available() means one or a few bytes are available, not an entire "message" (however "message" is defined for your use case.)

Mike Scheutzow gravatar image Mike Scheutzow  ( 2023-01-12 07:40:08 -0500 )edit

@Mike Scheutzow I would like to say link text I guess.

Additional question, You said that in my case, "message" is defiened(not one or two bytes). Does message here means ASCII?

daidalos99 gravatar image daidalos99  ( 2023-01-12 07:50:09 -0500 )edit

1 Answer

Sort by » oldest newest most voted
0

answered 2023-01-12 08:35:39 -0500

Mike Scheutzow gravatar image

Your code is missing required functionality.

With serial ports, bytes usually arrive just a few at a time. So you typically need to gather a complete "line" of characters into a buffer before you try to parse the data. That means the sender (the IMU for you) must mark the end of a line somehow (the characters '\r' or '\n' are often used for this purpose, but you need to figure out what your IMU does)

Make sure your code handles the case where both the end of one line and the start of the next line are returned in the same read() call.

edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2023-01-12 01:51:40 -0500

Seen: 134 times

Last updated: Jan 12 '23