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);
  }
}
Exemplo n.º 2
0
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++;
  }
}