Revision history [back]

It seems quaternions and IMU message sending work just fine on the Arduino, but some of the tf math is missing. Rosserial only implements createQuaternionFromYaw, but I need createQuaternionFromRPY. Fortunately, this algorithm is well documented elsewhere, as the rosserial_arduino author noted, so all I had to add to my code was:

static geometry_msgs::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw) {
    // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Source_Code
    // http://docs.ros.org/api/geometry_msgs/html/msg/Quaternion.html
    geometry_msgs::Quaternion q;
    double t0 = cos(yaw * 0.5);
    double t1 = sin(yaw * 0.5);
    double t2 = cos(roll * 0.5);
    double t3 = sin(roll * 0.5);
    double t4 = cos(pitch * 0.5);
    double t5 = sin(pitch * 0.5);
    q.w = t0 * t2 * t4 + t1 * t3 * t5;
    q.x = t0 * t3 * t4 - t1 * t2 * t5;
    q.y = t0 * t2 * t5 + t1 * t3 * t4;
    q.z = t1 * t2 * t4 - t0 * t3 * t5;
    return q;
}

and then my IMU publisher looks like:

imu_msg.header.stamp = nh.now();
imu_msg.header.frame_id = base_link;
imu_msg.orientation = createQuaternionFromRPY(imu.roll_radians, imu.pitch_radians, imu.yaw_radians);
imu_msg.linear_acceleration.x = imu.accel.x;
imu_msg.linear_acceleration.y = imu.accel.y;
imu_msg.linear_acceleration.z = imu.accel.z;
imu_publisher.publish(&imu_msg);

I ran into some weird issues because I forgot to advertise my publisher with:

ros::Publisher imu_publisher = ros::Publisher("imu", &imu_msg);

and I think that was causing the Arduino to crash or hang, and I was confusing that with the IMU packet being too big, when it was actually just fine. But after I added that, I was able to subscript to /myarduino/imu with rostopic and see messages stream without issue.

It seems quaternions and IMU message sending work just fine on the Arduino, but some of the tf math is missing. Rosserial only implements createQuaternionFromYaw, but I need createQuaternionFromRPY. Fortunately, this algorithm is well documented elsewhere, as the rosserial_arduino author noted, so all I had to add to my code was:

static geometry_msgs::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw) {
    // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Source_Code
    // http://docs.ros.org/api/geometry_msgs/html/msg/Quaternion.html
    geometry_msgs::Quaternion q;
    double t0 = cos(yaw * 0.5);
    double t1 = sin(yaw * 0.5);
    double t2 = cos(roll * 0.5);
    double t3 = sin(roll * 0.5);
    double t4 = cos(pitch * 0.5);
    double t5 = sin(pitch * 0.5);
    q.w = t0 * t2 * t4 + t1 * t3 * t5;
    q.x = t0 * t3 * t4 - t1 * t2 * t5;
    q.y = t0 * t2 * t5 + t1 * t3 * t4;
    q.z = t1 * t2 * t4 - t0 * t3 * t5;
    return q;
}

and then my IMU publisher looks like:

imu_msg.header.stamp = nh.now();
imu_msg.header.frame_id = base_link;
imu_msg.orientation = createQuaternionFromRPY(imu.roll_radians, imu.pitch_radians, imu.yaw_radians);
imu_msg.linear_acceleration.x = imu.accel.x;
imu_msg.linear_acceleration.y = imu.accel.y;
imu_msg.linear_acceleration.z = imu.accel.z;
imu_publisher.publish(&imu_msg);

I ran into some weird issues because I forgot to advertise my publisher with:

ros::Publisher imu_publisher = ros::Publisher("imu", &imu_msg);

and I think that was causing the Arduino to crash or hang, and I was confusing that with the IMU packet being too big, when it was actually just fine. But after I added that, I was able to subscript subscribe to /myarduino/imu with rostopic and see IMU messages stream without issue.