Implemnt Gyrosensor in ROS

edit

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_);
}