Converting Python tf code to C++

edit

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 subscribe to /myarduino/imu with rostopic and see IMU messages stream without issue.