Hello all. I have a robot that I'm using with ROS2 and I need to send IMU data over Serial for it. In total, I have to send 10 values: 4 from the quaternion (qx,qy,qz,qw), 3 accel, 3 gryo. The quaternion data is from the DMP and type is float, while the accel and gyro are of type int. This is what the code looks like for sending the imu data:
case IMU_READ:
Serial.print(q.w);
Serial.print(" ");
Serial.print(q.x);
Serial.print(" ");
Serial.print(q.y);
Serial.print(" ");
Serial.print(q.z);
Serial.print(" ");
Serial.print(ax);
Serial.print(" ");
Serial.print(ay);
Serial.print(" ");
Serial.print(az);
Serial.print(" ");
Serial.print(gx);
Serial.print(" ");
Serial.print(gy);
Serial.print(" ");
Serial.println(gz);
break;
The entire code for the arduino is here: ros_arduino_bridge ros_arduino_bridge works absolutely fine, I've just configured it to use the IMU DMP through the MPU6050 library. Everything else related to the DMP works fine except the sending of the data over serial.
From the ROS side, I have to parse the data and store it. I am doing it with this function here.
void read_imu(double& qx, double& qy, double& qz, double& qw, int& ax, int& ay, int& az, int& gx, int& gy, int& gz)
{
std::string response = send_msg("i\r"); // returns the imu string data from the arduino
std::string delimiter = " ";
size_t start_pos = 0;
for (int i = 0; i < 9; ++i) {
size_t del_pos = response.find(delimiter, start_pos);
// Assign each value directly to its respective variable
switch (i) {
case 0:
try {
qx = std::stod(response.substr(start_pos, del_pos - start_pos).c_str());;
}
catch (const std::invalid_argument&)
{
std::cerr << "Argument is invalid\n";
throw;
}
break;
case 1: qy = std::stod(response.substr(start_pos, del_pos - start_pos).c_str()); break;
case 2: qz = std::stod(response.substr(start_pos, del_pos - start_pos).c_str()); break;
case 3: qw = std::stod(response.substr(start_pos, del_pos - start_pos).c_str()); break;
case 4: ax = std::stoi(response.substr(start_pos, del_pos - start_pos).c_str()); break;
case 5: ay = std::stoi(response.substr(start_pos, del_pos - start_pos).c_str()); break;
case 6: az = std::stoi(response.substr(start_pos, del_pos - start_pos).c_str()); break;
case 7: gx = std::stoi(response.substr(start_pos, del_pos - start_pos).c_str()); break;
case 8: gy = std::stoi(response.substr(start_pos, del_pos - start_pos).c_str()); break;
}
start_pos = del_pos + delimiter.length();
}
// Extract the last value
gz = std::atoi(response.substr(start_pos).c_str());
}
The problem is that the whole thing is very slow and either the Serial read times out or there is some data missing. I'm at a loss for how to speed it up or send data efficiently as I'm still new to this so any guidance is helpful. I'm using an Arduino Mega with a baud rate of 115200 (p.s I know ros_arduino_bridge is written for the UNO and not Mega, but the IMU bit I'm doing is independent of the ports and I will port it for the Mega once I get this working).
!