the argument scale of quoternion function "setRPY"
Hi
I want to use "setRPY"function and the arguments are the gyro sensor values. I want to convert the angle to quotarnion by this function.
then , I have something I want to confirm. Should I set the scale of the sensor value (angle) to [-2π , 2π]? I don't know the scale of the function's argument . please let me know the correct scale of the function.
My code is the following :
gyro_msg.vector.x = data.gxPI()/180; gyro_msg.vector.y = data.gyPI()/180; gyro_msg.vector.z = data.gz*PI()/180; gyro_pub.publish(gyro_msg);
sensor_msgs::Imu imu_msg; dt = (float) ( endTime - startTime ) * 0.000001; // μsec→sec
// integration
data.theta += dt * data.gz*PI()/180;
// range constraint
if(data.theta>=2PI()) data.theta -=2PI(); else if(data.theta<=2PI()) data.theta +=2PI();
imu_msg.header.stamp = ros::Time::now(); imu_msg.header.frame_id = "/base_link"; tf::Quaternion q; q.setRPY(0.0, 0.0, data.theta);
thank you