//YO DUMBBY, these are measured in degrees, not radians! (But i'll do the change here)
ihmc_msgs::ChestTrajectoryRosMessage moveChest(std::vector<double> angles,bool record){
/*std::cout<<"CHEST: ";
for(int i=0;i<3;i++){
std::cout<<" Joint "<<i+1<<": "<<angles[i]*(180/PI);
}
std::cout<<std::endl;*/
double high[3]={1.047,1.378,1.2217};
double low[3]={-1.047,-.7,-1.2217};
ihmc_msgs::ChestTrajectoryRosMessage msg;
ihmc_msgs::SO3TrajectoryPointRosMessage n2;
for(int i=0;i<3;i++){
angles[i]=(angles[i]>high[i]?high[i]:angles[i]);
angles[i]=(angles[i]<low[i]?low[i]:angles[i]);
}
std::vector<double> quat=eulerToQuaternion(angles);
n2.time=(moveSpeedOverRide==-1?gui.moveSpeed:moveSpeedOverRide);
n2.orientation.x=quat[0];
n2.orientation.y=quat[1];
n2.orientation.z=quat[2];
n2.orientation.w=quat[3];
n2.angular_velocity.x=0;n2.angular_velocity.z=0;n2.angular_velocity.y=0;
msg.taskspace_trajectory_points.push_back(n2);
msg.unique_id=uId++;
for(int i=0;i<3;i++)
gui.humanPositions[i+17]=angles[i];
if(record)
gui.record("4: ",angles);
return msg;
}
int data_fusion(mpudata_t *mpu) {
quaternion_t dmpQuat;
vector3d_t dmpEuler;
quaternion_t magQuat;
quaternion_t unfusedQuat;
float deltaDMPYaw;
float deltaMagYaw;
float newMagYaw;
float newYaw;
dmpQuat[QUAT_W] = (float)mpu->rawQuat[QUAT_W];
dmpQuat[QUAT_X] = (float)mpu->rawQuat[QUAT_X];
dmpQuat[QUAT_Y] = (float)mpu->rawQuat[QUAT_Y];
dmpQuat[QUAT_Z] = (float)mpu->rawQuat[QUAT_Z];
quaternionNormalize(dmpQuat);
quaternionToEuler(dmpQuat, dmpEuler);
mpu->fusedEuler[VEC3_X] = dmpEuler[VEC3_X];
mpu->fusedEuler[VEC3_Y] = -dmpEuler[VEC3_Y];
mpu->fusedEuler[VEC3_Z] = 0;
eulerToQuaternion(mpu->fusedEuler, unfusedQuat);
deltaDMPYaw = -dmpEuler[VEC3_Z] + mpu->lastDMPYaw;
mpu->lastDMPYaw = dmpEuler[VEC3_Z];
magQuat[QUAT_W] = 0;
magQuat[QUAT_X] = mpu->calibratedMag[VEC3_X];
magQuat[QUAT_Y] = mpu->calibratedMag[VEC3_Y];
magQuat[QUAT_Z] = mpu->calibratedMag[VEC3_Z];
tilt_compensate(magQuat, unfusedQuat);
newMagYaw = -atan2f(magQuat[QUAT_Y], magQuat[QUAT_X]);
if (newMagYaw != newMagYaw) {
printf("newMagYaw NAN\n");
return -1;
}
if (newMagYaw < 0.0f)
newMagYaw = TWO_PI + newMagYaw;
newYaw = mpu->lastYaw + deltaDMPYaw;
if (newYaw > TWO_PI)
newYaw -= TWO_PI;
else if (newYaw < 0.0f)
newYaw += TWO_PI;
deltaMagYaw = newMagYaw - newYaw;
if (deltaMagYaw >= (float)M_PI)
deltaMagYaw -= TWO_PI;
else if (deltaMagYaw < -(float)M_PI)
deltaMagYaw += TWO_PI;
if (yaw_mixing_factor > 0)
newYaw += deltaMagYaw / yaw_mixing_factor;
if (newYaw > TWO_PI)
newYaw -= TWO_PI;
else if (newYaw < 0.0f)
newYaw += TWO_PI;
mpu->lastYaw = newYaw;
if (newYaw > (float)M_PI)
newYaw -= TWO_PI;
mpu->fusedEuler[VEC3_Z] = newYaw;
eulerToQuaternion(mpu->fusedEuler, mpu->fusedQuat);
return 0;
}
int main(int argc, char **argv)
{
// Initialize ROS
ros::init(argc, argv, "am_mpu9150_node");
ros::NodeHandle n;
ros::Publisher imu_pub = n.advertise<sensor_msgs::Imu>("imu", 1);
ros::Publisher imu_euler_pub = n.advertise<geometry_msgs::Vector3Stamped>("imu_euler", 1);
ros::Rate loop_rate(50);
sensor_msgs::Imu imu;
imu.header.frame_id = "imu";
geometry_msgs::Vector3Stamped euler;
euler.header.frame_id = "imu_euler";
// Init the sensor the values are hardcoded at the local_defaults.h file
int opt, len;
int i2c_bus = DEFAULT_I2C_BUS;
int sample_rate = DEFAULT_SAMPLE_RATE_HZ;
int yaw_mix_factor = DEFAULT_YAW_MIX_FACTOR;
int verbose = 0;
std::string accelFile;
std::string magFile;
// Parameters from ROS
ros::NodeHandle n_private("~");
int def_i2c_bus = 2;
n_private.param("i2cbus", i2c_bus, def_i2c_bus);
if (i2c_bus < MIN_I2C_BUS || i2c_bus > MAX_I2C_BUS)
{
ROS_ERROR("am_mpu9150: Imu Bus problem");
return -1;
}
int def_sample_rate = 50;
n_private.param("samplerate", sample_rate, def_sample_rate);
if (sample_rate < MIN_SAMPLE_RATE || sample_rate > MAX_SAMPLE_RATE)
{
ROS_ERROR("am_mpu9150: Sample rate problem");
return -1;
}
loop_rate = sample_rate;
int def_yaw_mix_factor = 0;
n_private.param("yawmixfactor", yaw_mix_factor, def_yaw_mix_factor);
if (yaw_mix_factor < 0 || yaw_mix_factor > 100)
{
ROS_ERROR("am_mpu9150: yawmixfactor problem");
return -1;
}
std::string defAccelFile = "./accelcal.txt";
n_private.param("accelfile", accelFile, defAccelFile);
std::string defMagFile = "./magcal.txt";
n_private.param("magfile", magFile, defMagFile);
unsigned long loop_delay;
mpudata_t mpu;
// Initialize the MPU-9150
mpu9150_set_debug(verbose);
if (mpu9150_init(i2c_bus, sample_rate, yaw_mix_factor))
{
ROS_ERROR("am_mpu9150::Failed init!");
exit(1);
}
//load_calibration(0, accelFile);
//load_calibration(1, magFile);
memset(&mpu, 0, sizeof(mpudata_t));
vector3d_t e;
quaternion_t q;
while (ros::ok())
{
std::stringstream ss;
if (mpu9150_read(&mpu) == 0)
{
// ROTATE The angles correctly...
quaternionToEuler(mpu.fusedQuat, e);
e[VEC3_Z] = -e[VEC3_Z];
eulerToQuaternion(e, q);
// FUSED
imu.header.stamp = ros::Time::now();
imu.orientation.x = q[QUAT_X];
imu.orientation.y = q[QUAT_Y];
imu.orientation.z = q[QUAT_Z];
imu.orientation.w = q[QUAT_W];
// GYRO
double gx = mpu.rawGyro[VEC3_X];
double gy = mpu.rawGyro[VEC3_Y];
double gz = mpu.rawGyro[VEC3_Z];
gx = gx * gyroScaleX;
gy = gy * gyroScaleY;
gz = gz * gyroScaleZ;
imu.angular_velocity.x = gx;
imu.angular_velocity.y = gy;
imu.angular_velocity.z = gz;
// ACCEL
imu.linear_acceleration.x = mpu.calibratedAccel[VEC3_X];
imu.linear_acceleration.y = mpu.calibratedAccel[VEC3_Y];
imu.linear_acceleration.z = mpu.calibratedAccel[VEC3_Z];
// EULER
euler.header.stamp = imu.header.stamp;
euler.vector.x = mpu.fusedEuler[VEC3_Y];
euler.vector.y = mpu.fusedEuler[VEC3_X];
euler.vector.z = -mpu.fusedEuler[VEC3_Z];
// Publish the messages
imu_pub.publish(imu);
imu_euler_pub.publish(euler);
//ROS_INFO("y=%f, p=%f, r=%f", euler.vector.z, euler.vector.y, euler.vector.x);
}
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
void ARDrone_Imu::PubIMU()
{
if (imu_pub.getNumSubscribers() == 0 && vo_pub.getNumSubscribers() == 0)
return;
sensor_msgs::Imu msg;
msg.header.stamp = rtime;
msg.angular_velocity_covariance[0] = -1;
for (int i = 0; i < 9; i++)
msg.linear_acceleration_covariance[i] = 0;
msg.linear_acceleration_covariance[0] = 0.25;
msg.linear_acceleration_covariance[4] = 0.25;
msg.linear_acceleration_covariance[8] = 0.25;
geometry_msgs::Quaternion q = eulerToQuaternion(degreeToRadian(rotx),
degreeToRadian(roty),
degreeToRadian(rotz));
for (int i = 0; i < 9; i++)
msg.orientation_covariance[i] = 0;
double zod = degreeToRadian(0.1);
msg.orientation_covariance[0] = zod;
msg.orientation_covariance[4] = zod;
msg.orientation_covariance[8] = zod;
msg.orientation = q;
geometry_msgs::Vector3 av;
geometry_msgs::Vector3 la;
la.x = accx;
la.y = accy;
la.z = accz;
av.x = rotx / dt;
av.y = roty / dt;
av.z = rotz / dt;
msg.angular_velocity = av;
msg.linear_acceleration = la;
nav_msgs::Odometry om;
om.header.stamp = rtime;
om.pose.pose.position.x = linx;
om.pose.pose.position.y = liny;
om.pose.pose.position.z = linz;
om.pose.pose.orientation = eulerToQuaternion(degreeToRadian(magx),
degreeToRadian(magy),
degreeToRadian(magz));
double c = degreeToRadian(6.0); // mag covarience..
double x,y,z;
x = y = 99999; // no idea where it is. :/
z = 10;
// todo: get a GPS device on here to fix that!
for (int i = 0; i < 36; i++)
om.pose.covariance[i] = 0;
om.pose.covariance[0] = x;
om.pose.covariance[7] = y;
om.pose.covariance[14] = z;
om.pose.covariance[21] = c;
om.pose.covariance[28] = c;
om.pose.covariance[35] = c;
imu_pub.publish(msg);
vo_pub.publish(om);
}
