Esempi in C++ (Cpp) per angular_velocity_I

Esempio n. 1

File: gazebo_imu_plugin.cpp Progetto: ruffsl/rotors_simulator

// This gets called by the world update start event.
void GazeboImuPlugin::OnUpdate(const common::UpdateInfo& _info) {
    common::Time current_time  = world_->GetSimTime();
    double dt = (current_time - last_time_).Double();
    last_time_ = current_time;
    double t = current_time.Double();

    math::Pose T_W_I = link_->GetWorldPose(); //TODO(burrimi): Check tf.
    math::Quaternion C_W_I = T_W_I.rot;

    math::Vector3 velocity_current_W = link_->GetWorldLinearVel();

    // link_->GetRelativeLinearAccel() does not work sometimes. Returns only 0.
    // TODO For an accurate simulation, this might have to be fixed. Consider the
    //      time delay introduced by this numerical derivative, for example.
    math::Vector3 acceleration = (velocity_current_W - velocity_prev_W_) / dt;
    math::Vector3 acceleration_I =
        C_W_I.RotateVectorReverse(acceleration - gravity_W_);
    math::Vector3 angular_vel_I = link_->GetRelativeAngularVel();

    Eigen::Vector3d linear_acceleration_I(acceleration_I.x,
                                          acceleration_I.y,
                                          acceleration_I.z);
    Eigen::Vector3d angular_velocity_I(angular_vel_I.x,
                                       angular_vel_I.y,
                                       angular_vel_I.z);

    addNoise(&linear_acceleration_I, &angular_velocity_I, dt);

    // Fill IMU message.
    imu_message_.header.stamp.sec = current_time.sec;
    imu_message_.header.stamp.nsec = current_time.nsec;

    // TODO(burrimi): Add orientation estimator.
    imu_message_.orientation.w = 1;
    imu_message_.orientation.x = 0;
    imu_message_.orientation.y = 0;
    imu_message_.orientation.z = 0;
//  imu_message_.orientation.w = C_W_I.w;
//  imu_message_.orientation.x = C_W_I.x;
//  imu_message_.orientation.y = C_W_I.y;
//  imu_message_.orientation.z = C_W_I.z;

    imu_message_.linear_acceleration.x = linear_acceleration_I[0];
    imu_message_.linear_acceleration.y = linear_acceleration_I[1];
    imu_message_.linear_acceleration.z = linear_acceleration_I[2];
    imu_message_.angular_velocity.x = angular_velocity_I[0];
    imu_message_.angular_velocity.y = angular_velocity_I[1];
    imu_message_.angular_velocity.z = angular_velocity_I[2];

    imu_pub_.publish(imu_message_);

    velocity_prev_W_ = velocity_current_W;
}

Esempio n. 2

File: gazebo_imu_plugin.cpp Progetto: 2016UAVClass/sitl_gazebo

// This gets called by the world update start event.
void GazeboImuPlugin::OnUpdate(const common::UpdateInfo& _info) {
  common::Time current_time  = world_->GetSimTime();
  double dt = (current_time - last_time_).Double();
  last_time_ = current_time;
  double t = current_time.Double();

  math::Pose T_W_I = link_->GetWorldPose(); //TODO(burrimi): Check tf.
  math::Quaternion C_W_I = T_W_I.rot;

  // Copy math::Quaternion to gazebo::msgs::Quaternion
  gazebo::msgs::Quaternion* orientation = new gazebo::msgs::Quaternion();
  orientation->set_x(C_W_I.x);
  orientation->set_y(C_W_I.y);
  orientation->set_z(C_W_I.z);
  orientation->set_w(C_W_I.w);








#if GAZEBO_MAJOR_VERSION < 5
  math::Vector3 velocity_current_W = link_->GetWorldLinearVel();
  // link_->GetRelativeLinearAccel() does not work sometimes with old gazebo versions.
  // TODO For an accurate simulation, this might have to be fixed. Consider the
  // This issue is solved in gazebo 5.
  math::Vector3 acceleration = (velocity_current_W - velocity_prev_W_) / dt;
  math::Vector3 acceleration_I =
      C_W_I.RotateVectorReverse(acceleration - gravity_W_);

  velocity_prev_W_ = velocity_current_W;
#else
  math::Vector3 acceleration_I = link_->GetRelativeLinearAccel() - C_W_I.RotateVectorReverse(gravity_W_);
#endif

  math::Vector3 angular_vel_I = link_->GetRelativeAngularVel();

  Eigen::Vector3d linear_acceleration_I(acceleration_I.x,
                                        acceleration_I.y,
                                        acceleration_I.z);
  Eigen::Vector3d angular_velocity_I(angular_vel_I.x,
                                     angular_vel_I.y,
                                     angular_vel_I.z);

  addNoise(&linear_acceleration_I, &angular_velocity_I, dt);

  // Copy Eigen::Vector3d to gazebo::msgs::Vector3d
  gazebo::msgs::Vector3d* linear_acceleration = new gazebo::msgs::Vector3d();
  linear_acceleration->set_x(linear_acceleration_I[0]);
  linear_acceleration->set_y(linear_acceleration_I[1]);
  linear_acceleration->set_z(linear_acceleration_I[2]);

  // Copy Eigen::Vector3d to gazebo::msgs::Vector3d
  gazebo::msgs::Vector3d* angular_velocity = new gazebo::msgs::Vector3d();
  angular_velocity->set_x(angular_velocity_I[0]);
  angular_velocity->set_y(angular_velocity_I[1]);
  angular_velocity->set_z(angular_velocity_I[2]);

  // Fill IMU message.
  // ADD HEaders
  // imu_message_.header.stamp.sec = current_time.sec;
  // imu_message_.header.stamp.nsec = current_time.nsec;

  // TODO(burrimi): Add orientation estimator.
  // imu_message_.orientation.w = 1;
  // imu_message_.orientation.x = 0;
  // imu_message_.orientation.y = 0;
  // imu_message_.orientation.z = 0;
  
  imu_message_.set_allocated_orientation(orientation);
  imu_message_.set_allocated_linear_acceleration(linear_acceleration);
  imu_message_.set_allocated_angular_velocity(angular_velocity);

  imu_pub_->Publish(imu_message_);
}