Error getting Imu via ros serial
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.data = ser.read(ser.available());
//cout << sensor.data << endl;
imu.header.stamp = ros::Time::now();
transformStamped.header.stamp = ros::Time::now();
vector<string> result = split(sensor, ',');
cout << result.size() << endl;
// float quaternion_w = atof(result[4].c_str());
// float quaternion_x = atof(result[3].c_str());
// float quaternion_y = atof(result[2].c_str());
// float quaternion_z = atof(result[1].c_str());
// float angular_velocity_x = atof(result[5].c_str());
// float angular_velocity_y = atof(result[6].c_str());
// float angular_velocity_z = atof(result[7].c_str());
// float linear_acceleration_x = atof(result[8].c_str());
// float linear_acceleration_y = atof(result[9].c_str());
// float linear_acceleration_z = atof(result[10].c_str());
// float distance_x = atof(result[11].c_str());
// float distance_y = atof(result[12].c_str());
// float distance_z = atof(result[13].c_str());
// q.setW(quaternion_w);
// q.setX(quaternion_x);
// q.setY(quaternion_y);
// q.setZ(quaternion_z);
// q.normalize();
// imu.orientation.w = q.w();
// imu.orientation.x = q.x();
// imu.orientation.y = q.y();
// imu.orientation.z = q.z();
// imu.linear_acceleration.x = linear_acceleration_x;
// imu.linear_acceleration.y = linear_acceleration_y;
// imu.linear_acceleration.z = linear_acceleration_z;
// imu.angular_velocity.x = angular_velocity_x;
// imu.angular_velocity.y = angular_velocity_y;
// imu.angular_velocity.z = angular_velocity_z;
// // transformStamped.transform.translation.x = distance_x;
// // transformStamped.transform.translation.y = distance_y;
// // transformStamped.transform.translation.z = distance_z;
// transformStamped.transform.translation.x = 0;
// transformStamped.transform.translation.y = 0;
// transformStamped.transform.translation.z = 0;
// transformStamped.transform.rotation.w = q.w();
// transformStamped.transform.rotation.x = q.x();
// transformStamped.transform.rotation.y = q.y();
// transformStamped.transform.rotation.z = q.z();
// cout << imu << endl;
// ROS_INFO_STREAM("Read: " << result.data);
//imu_pub.publish(imu);
//br.sendTransform(transformStamped);
//br.sendTransform(tf::StampedTransform(transform, ros_cloud.header.stamp, "map", "map_child"));
cnt++;
}
loop_rate.sleep();
}
return 0;
}
Asked by daidalos99 on 2023-01-12 02:51:40 UTC
Answers
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.
Asked by Mike Scheutzow on 2023-01-12 09:35:39 UTC
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.)Asked by Mike Scheutzow on 2023-01-12 08:40:08 UTC
@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?
Asked by daidalos99 on 2023-01-12 08:50:09 UTC