int main(int argc, char** argv) { ros::init(argc, argv, "i2cnode"); ros::NodeHandle nh; ros::NodeHandle n("~"); double accRate, gyroRate, magRate, barRate, avrRate, pubRate; ROS_INFO("fmAirframe : Reading parameters..."); n.param<double> ("accRate", accRate, 100); n.param<double> ("gyroRate", gyroRate, 100); n.param<double> ("magRate", magRate, 50); n.param<double> ("barRate", barRate, 25); n.param<double> ("avrRate", avrRate, 50); n.param<double> ("pubRate", pubRate, 50); estimator = new kalman(nh, n); ROS_INFO("fmAirframe : Advertising topics..."); accPublisher = nh.advertise<fmMsgs::accelerometer>("/accData", 1); gyroPublisher = nh.advertise<fmMsgs::gyroscope>("/gyroData", 1); magPublisher = nh.advertise<fmMsgs::magnetometer>("/magData", 1); statePublisher = nh.advertise<fmMsgs::airframeState>("/airframeState", 1); radioPublisher = nh.advertise<fmMsgs::airframeControl>("/radioData", 1); batteryPublisher = nh.advertise<fmMsgs::battery>("/batteryData", 1); pitotPublisher = nh.advertise<fmMsgs::airSpeed>("/pitotData", 1); rangePublisher = nh.advertise<fmMsgs::altitude>("/altData", 1); i2cfile i2c(3); ROS_INFO("fmAirframe : Starting hardware interfaces..."); avr myAVR(&i2c, &nh, ros::Rate(avrRate), &avrDataCallback); adxl345 myAcc(&i2c, &nh, ros::Rate(accRate), &accDataCallback); hmc5883l myMag(&i2c, &nh, ros::Rate(magRate), &magDataCallback); bmp085 myBar(&i2c, &nh, ros::Rate(barRate), &barDataCallback); itg3200 myGyro(&i2c, &nh, ros::Rate(gyroRate), &gyroDataCallback); myAvr = &myAVR; ROS_INFO("fmAirframe : Subscribint to topics..."); servoSubscriber = nh.subscribe("/servoData", 1, avrSetControlsCallback); ros::Timer pub_timer = nh.createTimer(ros::Duration(1/pubRate), pubCallback); ROS_INFO("fmAirframe : Spinning..."); ros::spin(); return 0; }
int main(int argc, char** argv) { bar myBar(1); return 0; }