I use the following api from the robotino: here
Also i write my own header and my own cpp-File and include it on the robotino_node
#include "GyroROS.h"
GyroROS::GyroROS()
{
gyroscope_pub_ = nh_.advertise<sensor_msgs::Imu>("gyroscope",1, true);
}
GyroROS::~GyroROS()
{
gyroscope_pub_.shutdown();
}
void GyroROS::setTimeStamp(ros::Time stamp)
{
stamp_ = stamp;
}
void GyroROS::gyroscopeEvent(float angle, float anglevel)
{
XAngle = 0.0;
YAngle = 0.0;
ZAngle = angle;
quat = Eigen::AngleAxisf(XAngle, Eigen::Vector3f::UnitX())
* Eigen::AngleAxisf(YAngle, Eigen::Vector3f::UnitY())
* Eigen::AngleAxisf(ZAngle, Eigen::Vector3f::UnitZ());
gyro_msg_.orientation.x = quat.x();
gyro_msg_.orientation.y = quat.y();
gyro_msg_.orientation.z = quat.z();
gyro_msg_.orientation.w = quat.w();
gyro_msg_.angular_velocity.z = -anglevel;
gyro_msg_.header.stamp = stamp_;
gyro_msg_.header.frame_id = "base_link";
// Covariance Matrix in Orientation
gyro_msg_.orientation_covariance[0] = 0.0;
gyro_msg_.orientation_covariance[4] = 0.0;
gyro_msg_.orientation_covariance[8] = 0.0;
// Covariance Matrix in Angule Velocity
gyro_msg_.angular_velocity_covariance[0] = 0.0;
gyro_msg_.angular_velocity_covariance[4] = 0.0;
gyro_msg_.angular_velocity_covariance[8] = 0.0;
gyroscope_pub_.publish(gyro_msg_);
}