tf::Quaternion LaserTransform::getQuaternion()
{
float x = 0.0, y = 0.0, z = 0.0, w = 0.0;
int16_t ix = 0, iy = 0, iz = 0, iw = 0;
if (is_imu_connected)
imu_get_quaternion(&imu, &x, &y, &z, &w);
if (is_imu_v2_connected)
{
imu_v2_get_quaternion(&imu_v2, &ix, &iy, &iz, &iw);
x = ix / 16383.0;
y = iy / 16383.0;
z = iz / 16383.0;
w = iw / 16383.0;
}
//-------
float yaw = atan2(2.0*(x*y + w*z), pow(w,2)+pow(x,2)-pow(y,2)-pow(z,2));
float pitch = -asin(2.0*(w*y - x*z));
float roll = -atan2(2.0*(y*z + w*x), -(pow(w,2)-pow(x,2)-pow(y,2)+pow(z,2)));
std::cout << "y:" << rad2deg(yaw) << ":: p:" << rad2deg(pitch) << ":: r:" << rad2deg(roll) << std::endl;
//-------
tf::Quaternion q;
q.setRPY(roll, pitch, yaw);
return q;
}
void TinkerforgeSensors::publishImuMessage(SensorDevice *sensor)
{
int16_t acc_x, acc_y, acc_z;
int16_t mag_x, mag_y, mag_z;
int16_t ang_x, ang_y, ang_z;
int16_t temp;
float x = 0.0, y = 0.0, z = 0.0, w = 0.0;
int16_t ix = 0, iy = 0, iz = 0, iw = 0;
ros::Time current_time = ros::Time::now();
tf::TransformBroadcaster tf_broadcaster;
if (sensor != NULL)
{
sensor_msgs::Imu imu_msg;
// for the conversions look at rep 103 http://www.ros.org/reps/rep-0103.html
// for IMU v1 http://www.tinkerforge.com/de/doc/Software/Bricks/IMU_Brick_C.html#imu-brick-c-api
// for IMU v2 http://www.tinkerforge.com/de/doc/Software/Bricks/IMUV2_Brick_C.html#imu-v2-brick-c-api
if (sensor->getType() == IMU_DEVICE_IDENTIFIER)
{
// check if imu is initialized
//if (ros::Time::now().sec - imu_init_time.sec < 3)
// return;
//else
// imu_set_convergence_speed(&imu, imu_convergence_speed);
//
imu_get_quaternion((IMU*)sensor->getDev(), &x, &y, &z, &w);
imu_get_all_data((IMU*)sensor->getDev(), &acc_x, &acc_y, &acc_z, &mag_x, &mag_y,
&mag_z, &ang_x, &ang_y, &ang_z, &temp);
ang_x = ang_x / 14.375;
ang_y = ang_y / 14.375;
ang_z = ang_z / 14.375;
acc_x = (acc_x/1000.0)*9.80605;
acc_y = (acc_y/1000.0)*9.80605;
acc_z = (acc_z/1000.0)*9.80605;
}
else if (sensor->getType() == IMU_V2_DEVICE_IDENTIFIER)
{
imu_v2_get_quaternion((IMUV2*)sensor->getDev(), &ix, &iy, &iz, &iw);
x = ix / 16383.0;
y = iy / 16383.0;
z = iz / 16383.0;
w = iw / 16383.0;
imu_v2_get_linear_acceleration((IMUV2*)sensor->getDev(), &acc_x, &acc_y, &acc_z);
imu_v2_get_angular_velocity((IMUV2*)sensor->getDev(), &ang_x, &ang_y, &ang_z);
ang_x = ang_x * 16;
ang_y = ang_y * 16;
ang_z = ang_z * 16;
acc_x = acc_x * 100;
acc_y = acc_y * 100;
acc_z = acc_z * 100;
}
else
{
return;
}
// message header
imu_msg.header.seq = sensor->getSeq();
imu_msg.header.stamp = current_time;
imu_msg.header.frame_id = sensor->getFrame();
//TODO adapt values for IMU v2
imu_msg.orientation.x = w;
imu_msg.orientation.y = z*-1;
imu_msg.orientation.z = y;
imu_msg.orientation.w = x*-1;
// orientation_covariance
boost::array<const double, 9> oc =
{ 0.1, 0.1, 0.1,
0.1, 0.1, 0.1,
0.1, 0.1, 0.1};
imu_msg.orientation_covariance = oc;
// velocity from °/14.375 to rad/s
imu_msg.angular_velocity.x = deg2rad(ang_x);
imu_msg.angular_velocity.y = deg2rad(ang_y);
imu_msg.angular_velocity.z = deg2rad(ang_z);
// velocity_covariance
boost::array<const double, 9> vc =
{ 0.1, 0.1, 0.1,
0.1, 0.1, 0.1,
0.1, 0.1, 0.1};
imu_msg.angular_velocity_covariance = vc;
// acceleration from mG to m/s²
imu_msg.linear_acceleration.x = acc_x;
imu_msg.linear_acceleration.y = acc_y;
imu_msg.linear_acceleration.z = acc_z;
// linear_acceleration_covariance
boost::array<const double, 9> lac =
{ 0.1, 0.1, 0.1,
0.1, 0.1, 0.1,
0.1, 0.1, 0.1};
imu_msg.linear_acceleration_covariance = lac;
sensor->getPub().publish(imu_msg);
}
}
void LaserTransform::publishImuMessage(ros::Publisher *pub_message)
{
int16_t acc_x, acc_y, acc_z;
int16_t mag_x, mag_y, mag_z;
int16_t ang_x, ang_y, ang_z;
int16_t temp;
float x = 0.0, y = 0.0, z = 0.0, w = 0.0;
int16_t ix = 0, iy = 0, iz = 0, iw = 0;
static uint32_t seq = 0;
ros::Time current_time = ros::Time::now();
tf::TransformBroadcaster tf_broadcaster;
if (is_imu_connected || is_imu_v2_connected)
{
sensor_msgs::Imu imu_msg;
// for the conversions look at rep 103 http://www.ros.org/reps/rep-0103.html
// for IMU v1 http://www.tinkerforge.com/de/doc/Software/Bricks/IMU_Brick_C.html#imu-brick-c-api
// for IMU v2 http://www.tinkerforge.com/de/doc/Software/Bricks/IMUV2_Brick_C.html#imu-v2-brick-c-api
if (is_imu_connected)
{
// check if imu is initialized
/*if (ros::Time::now().sec - imu_init_time.sec < 3)
return;
else
imu_set_convergence_speed(&imu, imu_convergence_speed);
*/
imu_get_quaternion(&imu, &x, &y, &z, &w);
imu_get_all_data(&imu, &acc_x, &acc_y, &acc_z, &mag_x, &mag_y,
&mag_z, &ang_x, &ang_y, &ang_z, &temp);
ang_x = ang_x / 14.375;
ang_y = ang_y / 14.375;
ang_z = ang_z / 14.375;
acc_x = (acc_x/1000.0)*9.80605;
acc_y = (acc_y/1000.0)*9.80605;
acc_z = (acc_z/1000.0)*9.80605;
}
else
{
imu_v2_get_quaternion(&imu_v2, &ix, &iy, &iz, &iw);
x = ix / 16383.0;
y = iy / 16383.0;
z = iz / 16383.0;
w = iw / 16383.0;
imu_v2_get_linear_acceleration(&imu_v2, &acc_x, &acc_y, &acc_z);
imu_v2_get_angular_velocity(&imu_v2, &ang_x, &ang_y, &ang_z);
ang_x = ang_x * 16;
ang_y = ang_y * 16;
ang_z = ang_z * 16;
acc_x = acc_x * 100;
acc_y = acc_y * 100;
acc_z = acc_z * 100;
}
// message header
imu_msg.header.seq = seq;
imu_msg.header.stamp = current_time;
imu_msg.header.frame_id = "base_link";
//TODO adapt values for IMU v2
imu_msg.orientation.x = w;
imu_msg.orientation.y = z*-1;
imu_msg.orientation.z = y;
imu_msg.orientation.w = x*-1;
// orientation_covariance
boost::array<const double, 9> oc =
{ 0.1, 0.1, 0.1,
0.1, 0.1, 0.1,
0.1, 0.1, 0.1};
imu_msg.orientation_covariance = oc;
// velocity from °/14.375 to rad/s
imu_msg.angular_velocity.x = deg2rad(ang_x);
imu_msg.angular_velocity.y = deg2rad(ang_y);
imu_msg.angular_velocity.z = deg2rad(ang_z);
// velocity_covariance
boost::array<const double, 9> vc =
{ 0.1, 0.1, 0.1,
0.1, 0.1, 0.1,
0.1, 0.1, 0.1};
imu_msg.angular_velocity_covariance = vc;
// acceleration from mG to m/s²
imu_msg.linear_acceleration.x = acc_x;
imu_msg.linear_acceleration.y = acc_y;
imu_msg.linear_acceleration.z = acc_z;
// linear_acceleration_covariance
boost::array<const double, 9> lac =
{ 0.1, 0.1, 0.1,
0.1, 0.1, 0.1,
0.1, 0.1, 0.1};
imu_msg.linear_acceleration_covariance = lac;
pub_message->publish(imu_msg);
seq++;
}
}
