int main(int argc, char *argv[])
{
int parameter;
char *sensor_name;
if (check_apm()) {
return 1;
}
if (argc < 2) {
printf("Enter parameter\n");
print_help();
return EXIT_FAILURE;
}
// prevent the error message
opterr=0;
while ((parameter = getopt(argc, argv, "i:h")) != -1) {
switch (parameter) {
case 'i': sensor_name = optarg; break;
case 'h': print_help(); return EXIT_FAILURE;
case '?': printf("Wrong parameter.\n");
print_help();
return EXIT_FAILURE;
}
}
imu = create_inertial_sensor(sensor_name);
if (!imu) {
printf("Wrong sensor name. Select: mpu or lsm\n");
return EXIT_FAILURE;
}
if (!imu->probe()) {
printf("Sensor not enable\n");
return EXIT_FAILURE;
}
//--------------------------- Network setup -------------------------------
sockfd = socket(AF_INET,SOCK_DGRAM,0);
servaddr.sin_family = AF_INET;
if (argc == 5) {
servaddr.sin_addr.s_addr = inet_addr(argv[3]);
servaddr.sin_port = htons(atoi(argv[4]));
} else {
servaddr.sin_addr.s_addr = inet_addr("127.0.0.1");
servaddr.sin_port = htons(7000);
}
//-------------------- IMU setup and main loop ----------------------------
imuSetup();
while(1)
imuLoop();
}
int main(int argc, char **argv)
{
/***********************/
/* Initialize The Node */
/***********************/
ros::init(argc, argv, "imu_lsm9ds1_handler");
ros::NodeHandle n;
ros::Publisher imu_pub = n.advertise<sensor_msgs::Imu>("imu_readings", 1000);
ros::Publisher mf_pub = n.advertise<sensor_msgs::MagneticField>("mag_readings", 1000);
//running rate = 30 Hz
ros::Rate loop_rate(30);
/*************************/
/* Initialize the Sensor */
/*************************/
printf("Selected: LSM9DS1\n");
imu = new LSM9DS1();
/***************/
/* Test Sensor */
/***************/
if (!imu->probe())
{
printf("Sensor not enabled\n");
return EXIT_FAILURE;
}
imuSetup();
while (ros::ok())
{
imuLoop();
sensor_msgs::Imu imu_msg;
sensor_msgs::MagneticField mf_msg;
init_imu_msg(&imu_msg);
init_mf_msg(&mf_msg);
update_imu_msg(&imu_msg, imu);
update_mf_msg(&mf_msg, imu);
imu_pub.publish(imu_msg);
mf_pub.publish(mf_msg);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
int main(int argc, char **argv)
{
// The parameter to this function is the running frequency
if(argc == 2)
{
if(atoi(argv[1]) > 0)
{
freq = atoi(argv[1]);
printf("Frequency selected : %d \n", freq);
}
else
{
printf("Frequency must be more than 0");
return 0;
}
}
/***********************/
/* Initialize The Node */
/***********************/
ros::init(argc, argv, "imu_mpu9250_handler");
ros::NodeHandle n;
ros::Publisher imu_pub = n.advertise<sensor_msgs::Imu>("imu_readings", 1000);
ros::Publisher mf_pub = n.advertise<sensor_msgs::MagneticField>("mag_readings", 1000);
ros::Rate loop_rate(freq);
/*************************/
/* Initialize the Sensor */
/*************************/
printf("Selected: MPU9250\n");
imu = new MPU9250();
/***************/
/* Test Sensor */
/***************/
if (!imu->probe())
{
printf("Sensor not enabled\n");
return EXIT_FAILURE;
}
imuSetup();
while (ros::ok())
{
//acquire data
imuLoop();
//create messages
sensor_msgs::Imu imu_msg;
sensor_msgs::MagneticField mf_msg;
//initialize messages with 0's
init_imu_msg(&imu_msg);
init_mf_msg(&mf_msg);
//put real measured values in the messages
update_imu_msg(&imu_msg, imu);
update_mf_msg(&mf_msg, imu);
//publish the messages on the topics
imu_pub.publish(imu_msg);
mf_pub.publish(mf_msg);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
